diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Dynamics/Thruster.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Dynamics/Thruster.cpp index 75de69be7..252996791 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Dynamics/Thruster.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Dynamics/Thruster.cpp @@ -85,8 +85,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Dynamics_Thruster(pybind11::module& Args: satellite_system (SatelliteSystem): The satellite system. guidance_law (GuidanceLaw): The guidance law used to compute the acceleration vector. - name (str): The name of the thruster. - + name (str, optional): The name of the thruster. Defaults to String.empty(). )doc" ) diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Maneuver.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Maneuver.cpp index 40bcafa30..d3b6c447c 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Maneuver.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Maneuver.cpp @@ -8,6 +8,8 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Flight_Maneuver(pybind11::module& aM using ostk::mathematics::curvefitting::Interpolator; + using ostk::physics::unit::Angle; + using ostk::astrodynamics::flight::Maneuver; class_( @@ -130,6 +132,43 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Flight_Maneuver(pybind11::module& aM Tabulated: The tabulated dynamics. )doc" ) + .def( + "calculate_mean_thrust_direction_and_maximum_angular_offset", + &Maneuver::calculateMeanThrustDirectionAndMaximumAngularOffset, + arg("local_orbital_frame_factory"), + R"doc( + Calculate the mean thrust direction in the Local Orbital Frame and its maximum angular offset w.r.t. the maneuver's thrust acceleration directions. + + Args: + local_orbital_frame_factory (LocalOrbitalFrameFactory): The local orbital frame factory. + + Returns: + Tuple[LocalOrbitalFrameDirection, Angle]: The mean thrust direction and its maximum angular offset. + )doc" + ) + .def( + "to_constant_local_orbital_frame_direction_maneuver", + &Maneuver::toConstantLocalOrbitalFrameDirectionManeuver, + arg("local_orbital_frame_factory"), + arg_v("maximum_allowed_angular_offset", Angle::Undefined(), "Angle.Undefined()"), + R"doc( + Create a maneuver with a constant thrust acceleration direction in the Local Orbital Frame. + + The new Maneuver contains the same states as the original Maneuver, but the thrust acceleration direction is + constant in the Local Orbital Frame. Said direction is the mean direction of the thrust acceleration directions + in the Local Orbital Frame of the original Maneuver. The thrust acceleration magnitude profile is the same as the original. + + If defined, a runtime error will be thrown if the maximum allowed angular offset between the original thrust acceleration direction + and the mean thrust direction is violated. + + Args: + local_orbital_frame_factory (LocalOrbitalFrameFactory): The local orbital frame factory. + maximum_allowed_angular_offset (Angle, optional): The maximum allowed angular offset to consider (if any). Defaults to Undefined. + + Returns: + Maneuver: The constant local orbital frame direction maneuver. + )doc" + ) .def_static( "constant_mass_flow_rate_profile", &Maneuver::ConstantMassFlowRateProfile, diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Profile.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Profile.cpp index dadd62dd8..6fdd7548a 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Profile.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Profile.cpp @@ -191,7 +191,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Flight_Profile(pybind11::module& aMo Constructor. Args: - orientation_profile (list[Tuple[Instant, Vector3d]]): The orientation profile. + orientation_profile (list[tuple[Instant, Vector3d]]): The orientation profile. axis (Profile.Axis): The axis. anti_direction (bool): True if the direction is flipped, False otherwise. Defaults to False. )doc", diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw.cpp index a971403cc..f7d0e8aa8 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw.cpp @@ -3,6 +3,7 @@ #include #include +#include #include using namespace pybind11; @@ -120,4 +121,5 @@ void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw(pybind11::module& aModule) // Add objects to "guidance_law" submodule OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_ConstantThrust(guidance_law); OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_QLaw(guidance_law); + OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_HeterogeneousGuidanceLaw(guidance_law); } diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/ConstantThrust.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/ConstantThrust.cpp index 778f9b513..01321b061 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/ConstantThrust.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/ConstantThrust.cpp @@ -18,7 +18,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_ConstantThrust(pybind11: aModule, "ConstantThrust", R"doc( - Constant Thrust, Constant Direction dynamics. + Constant Thrust, Constant Direction guidance law. )doc" ) @@ -78,17 +78,40 @@ inline void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_ConstantThrust(pybind11: "intrack", &ConstantThrust::Intrack, R"doc( - Create a constant thrust in the in-track direction. + Create a constant thrust guidance law in the in-track direction. Args: - satellite_system (SatelliteSystem): The satellite system. - velocity_direction (bool, optional): If True, the thrust is applied in the velocity direction. Otherwise, it is applied in the opposite direction. - frame (Frame, optional): The reference frame. + velocity_direction (bool, optional): If True, the thrust is applied in the velocity direction. Otherwise, it is applied in the opposite direction. Defaults to True. Returns: - ConstantThrust: The constant thrust. + ConstantThrust: The constant thrust guidance law in the in-track direction. )doc", - arg("velocity_direction") = true + arg_v("velocity_direction", true, "True") + ) + + .def_static( + "from_maneuver", + &ConstantThrust::FromManeuver, + R"doc( + Create a constant thrust guidance law from a maneuver. + + The local orbital frame maneuver's mean thrust direction is calculated and used to create a + constant thrust guidance law in said direction. + + If defined, a runtime error will be thrown if the maximum allowed angular offset between the original thrust acceleration + direction and the mean thrust direction is violated. + + Args: + maneuver (Maneuver): The maneuver. + local_orbital_frame_factory (LocalOrbitalFrameFactory): The local orbital frame factory. + maximum_allowed_angular_offset (Angle, optional): The maximum allowed angular offset to consider (if any). Defaults to Undefined. + + Returns: + ConstantThrust: The constant thrust guidance law. + )doc", + arg("maneuver"), + arg("local_orbital_frame_factory"), + arg_v("maximum_allowed_angular_offset", Angle::Undefined(), "Angle.Undefined()") ) ; diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/HeterogeneousGuidanceLaw.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/HeterogeneousGuidanceLaw.cpp new file mode 100644 index 000000000..2bd4b2186 --- /dev/null +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/HeterogeneousGuidanceLaw.cpp @@ -0,0 +1,104 @@ +/// Apache License 2.0 + +#include + +using namespace pybind11; + +using ostk::core::container::Array; +using ostk::core::container::Pair; +using ostk::core::type::Shared; + +using ostk::mathematics::object::Vector3d; + +using ostk::physics::coordinate::Frame; +using ostk::physics::time::Instant; +using ostk::physics::time::Interval; + +using ostk::astrodynamics::GuidanceLaw; +using ostk::astrodynamics::guidancelaw::HeterogeneousGuidanceLaw; + +inline void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_HeterogeneousGuidanceLaw(pybind11::module& aModule) +{ + class_>( + aModule, + "HeterogeneousGuidanceLaw", + R"doc( + A guidance law that sequences multiple guidance laws over specific time intervals. + + At each point in time, the applicable guidance law is selected and used to calculate + the thrust acceleration. Guidance laws don't need to be contiguous, and can be added + in any order. If the instant does not fall within any of the intervals, the thrust + acceleration is zero. The guidance law intervals must not intersect each other. + )doc" + ) + + .def( + init, Interval>>&>(), + arg_v("guidance_laws_with_intervals", Array, Interval>>::Empty(), "[]"), + R"doc( + Constructor. + + Args: + guidance_laws_with_intervals (list[tuple[GuidanceLaw, Interval]], optional): Array of tuples containing the guidance laws and their corresponding intervals. Defaults to empty array. + + Raises: + RuntimeError: If any interval is undefined, if the guidance law is null or if the interval intersects with an existing interval. + )doc" + ) + + .def( + "get_guidance_laws_with_intervals", + &HeterogeneousGuidanceLaw::getGuidanceLawsWithIntervals, + R"doc( + Get the guidance laws with their corresponding intervals. + + Returns: + list[tuple[GuidanceLaw, Interval]]: Array of tuples containing the guidance laws and their corresponding intervals. + )doc" + ) + + .def( + "add_guidance_law", + &HeterogeneousGuidanceLaw::addGuidanceLaw, + arg("guidance_law"), + arg("interval"), + R"doc( + Add a guidance law with its corresponding interval. + + Args: + guidance_law (GuidanceLaw): The guidance law to add. + interval (Interval): The interval during which the guidance law is active. + + Raises: + RuntimeError: If the interval is undefined, if the guidance law is null or if the interval intersects with an existing interval. + )doc" + ) + + .def( + "calculate_thrust_acceleration_at", + &HeterogeneousGuidanceLaw::calculateThrustAccelerationAt, + arg("instant"), + arg("position_coordinates"), + arg("velocity_coordinates"), + arg("thrust_acceleration"), + arg("output_frame"), + R"doc( + Calculate thrust acceleration at a given instant and state. + + Args: + instant (Instant): The instant. + position_coordinates (numpy.ndarray): The position coordinates. + velocity_coordinates (numpy.ndarray): The velocity coordinates. + thrust_acceleration (float): The thrust acceleration magnitude. + output_frame (Frame): The frame in which the acceleration is expressed. + + Returns: + numpy.ndarray: The acceleration vector at the provided coordinates. + )doc" + ) + + .def("__str__", &(shiftToString)) + .def("__repr__", &(shiftToString)) + + ; +} diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/Segment.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/Segment.cpp index c73482b06..b5fd6dbd5 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/Segment.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/Segment.cpp @@ -11,9 +11,11 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Segment(pybind11::module& using ostk::core::type::String; using ostk::physics::time::Duration; + using ostk::physics::unit::Angle; using ostk::astrodynamics::Dynamics; using ostk::astrodynamics::flight::Maneuver; + using ostk::astrodynamics::trajectory::LocalOrbitalFrameFactory; using ostk::astrodynamics::trajectory::Segment; using ostk::astrodynamics::trajectory::state::CoordinateSubset; using ostk::astrodynamics::trajectory::state::NumericalSolver; @@ -420,5 +422,39 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_Segment(pybind11::module& )doc" ) + .def_static( + "constant_local_orbital_frame_direction_maneuver", + &Segment::ConstantLocalOrbitalFrameDirectionManeuver, + arg("name"), + arg("event_condition"), + arg("thruster_dynamics"), + arg("dynamics"), + arg("numerical_solver"), + arg("local_orbital_frame_factory"), + arg_v("maximum_allowed_angular_offset", Angle::Undefined(), "Angle.Undefined()"), + R"doc( + Create a maneuvering segment that produces maneuvers with a constant direction in the local orbital frame. + + The provided thruster dynamics are used to solve the segment at first. The maneuvers produced by this segement solution + are then used to create a new thruster dynamics with a constant direction in the local orbital frame. This new thruster dynamics + is then used to actually solve the segment. + + If defined, a runtime error will be thrown if the maximum allowed angular offset between the original thruster dynamics + and the mean thrust direction is violated. + + Args: + name (str): The name of the segment. + event_condition (EventCondition): The event condition. + thruster_dynamics (ThrusterDynamics): The thruster dynamics. + dynamics (Dynamics): The dynamics. + numerical_solver (NumericalSolver): The numerical solver. + local_orbital_frame_factory (LocalOrbitalFrameFactory): The local orbital frame factory. + maximum_allowed_angular_offset (Angle, optional): The maximum allowed angular offset to consider (if any). Defaults to Angle.undefined(). + + Returns: + Segment: The maneuver segment. + )doc" + ) + ; } diff --git a/bindings/python/test/dynamics/test_thruster.py b/bindings/python/test/dynamics/test_thruster.py index 752cadbf0..4506ba7e3 100644 --- a/bindings/python/test/dynamics/test_thruster.py +++ b/bindings/python/test/dynamics/test_thruster.py @@ -22,7 +22,6 @@ from ostk.astrodynamics.trajectory import State from ostk.astrodynamics.flight.system import PropulsionSystem from ostk.astrodynamics.flight.system import SatelliteSystem -from ostk.astrodynamics import Dynamics from ostk.astrodynamics.dynamics import Thruster from ostk.astrodynamics.guidance_law import ConstantThrust @@ -96,11 +95,27 @@ def state(coordinate_broker: CoordinateBroker) -> State: class TestThruster: - def test_constructors(self, dynamics: Thruster): - assert dynamics is not None - assert isinstance(dynamics, Thruster) - assert isinstance(dynamics, Dynamics) - assert dynamics.is_defined() + def test_constructors( + self, + guidance_law: ConstantThrust, + satellite_system: SatelliteSystem, + ): + thrusters = [ + Thruster( + satellite_system=satellite_system, + guidance_law=guidance_law, + ), + Thruster( + satellite_system=satellite_system, + guidance_law=guidance_law, + name="A name", + ), + ] + + for thruster in thrusters: + assert thruster is not None + assert isinstance(thruster, Thruster) + assert thruster.is_defined() def test_getters(self, dynamics: Thruster): assert dynamics.get_satellite_system() is not None diff --git a/bindings/python/test/flight/test_maneuver.py b/bindings/python/test/flight/test_maneuver.py index ef1b038db..083283a1b 100644 --- a/bindings/python/test/flight/test_maneuver.py +++ b/bindings/python/test/flight/test_maneuver.py @@ -4,16 +4,18 @@ import numpy as np - from ostk.physics.time import Instant from ostk.physics.time import Interval from ostk.physics.time import Duration from ostk.physics.coordinate import Frame from ostk.physics.unit import Mass +from ostk.physics.unit import Angle +from ostk.astrodynamics.dynamics import Tabulated as TabulatedDynamics from ostk.astrodynamics.flight import Maneuver +from ostk.astrodynamics.trajectory import LocalOrbitalFrameDirection +from ostk.astrodynamics.trajectory import LocalOrbitalFrameFactory from ostk.astrodynamics.trajectory import State -from ostk.astrodynamics.dynamics import Tabulated as TabulatedDynamics from ostk.astrodynamics.trajectory.state import CoordinateSubset from ostk.astrodynamics.trajectory.state.coordinate_subset import CartesianPosition from ostk.astrodynamics.trajectory.state.coordinate_subset import CartesianVelocity @@ -184,12 +186,44 @@ def test_to_tabulated_dynamics( assert tabulated_dynamics.access_frame() == frame + def test_calculate_mean_thrust_direction_and_maximum_angular_offset( + self, + maneuver: Maneuver, + ): + mean_thrust_direction_and_maximum_angular_offset = ( + maneuver.calculate_mean_thrust_direction_and_maximum_angular_offset( + local_orbital_frame_factory=LocalOrbitalFrameFactory.TNW(Frame.GCRF()), + ) + ) + assert isinstance( + mean_thrust_direction_and_maximum_angular_offset[0], + LocalOrbitalFrameDirection, + ) + assert isinstance(mean_thrust_direction_and_maximum_angular_offset[1], Angle) + + def test_to_constant_local_orbital_frame_direction_maneuver( + self, + maneuver: Maneuver, + ): + maneuver: Maneuver = maneuver.to_constant_local_orbital_frame_direction_maneuver( + local_orbital_frame_factory=LocalOrbitalFrameFactory.TNW(Frame.GCRF()), + ) + assert maneuver.is_defined() + + maneuver_with_maximum_allowed_angular_offset: Maneuver = ( + maneuver.to_constant_local_orbital_frame_direction_maneuver( + local_orbital_frame_factory=LocalOrbitalFrameFactory.TNW(Frame.GCRF()), + maximum_allowed_angular_offset=Angle.degrees(180.0), + ) + ) + assert maneuver_with_maximum_allowed_angular_offset.is_defined() + def test_from_constant_mass_flow_rate( self, states: list[State], ): mass_flow_rate: float = -1.0e-5 - maneuver = Maneuver.constant_mass_flow_rate_profile( + maneuver: Maneuver = Maneuver.constant_mass_flow_rate_profile( states=states, mass_flow_rate=mass_flow_rate, ) diff --git a/bindings/python/test/guidance_law/test_constant_thrust.py b/bindings/python/test/guidance_law/test_constant_thrust.py index 9a9a453e9..33c78e8ca 100644 --- a/bindings/python/test/guidance_law/test_constant_thrust.py +++ b/bindings/python/test/guidance_law/test_constant_thrust.py @@ -2,16 +2,26 @@ import pytest -from ostk.physics.time import Instant +import numpy as np + from ostk.physics.time import DateTime +from ostk.physics.time import Duration +from ostk.physics.time import Instant from ostk.physics.time import Scale +from ostk.physics.unit import Angle from ostk.physics.coordinate import Frame from ostk.astrodynamics.trajectory import LocalOrbitalFrameFactory from ostk.astrodynamics.trajectory import LocalOrbitalFrameDirection from ostk.astrodynamics import GuidanceLaw +from ostk.astrodynamics.flight import Maneuver from ostk.astrodynamics.guidance_law import ConstantThrust +from ostk.astrodynamics.trajectory import State +from ostk.astrodynamics.trajectory.state import CoordinateSubset +from ostk.astrodynamics.trajectory.state.coordinate_subset import CartesianPosition +from ostk.astrodynamics.trajectory.state.coordinate_subset import CartesianVelocity +from ostk.astrodynamics.trajectory.state.coordinate_subset import CartesianAcceleration @pytest.fixture @@ -54,6 +64,64 @@ def frame() -> Frame: return Frame.GCRF() +@pytest.fixture +def instants() -> list[Instant]: + return [ + Instant.J2000(), + Instant.J2000() + Duration.seconds(30.0), + Instant.J2000() + Duration.seconds(35.0), + Instant.J2000() + Duration.seconds(37.0), + ] + + +@pytest.fixture +def acceleration_profile() -> list[np.ndarray]: + return [ + np.array([1.0e-3, 0.0e-3, 0.0e-3]), + np.array([0.0e-3, 1.0e-3, 0.0e-3]), + np.array([0.0e-3, 0.0e-3, 1.0e-3]), + np.array([1.0e-3, 1.0e-3, 1.0e-3]), + ] + + +@pytest.fixture +def mass_flow_rate_profile() -> list[float]: + return [-1.0e-5, -1.1e-5, -0.9e-5, -1.0e-5] + + +@pytest.fixture +def coordinate_subsets() -> list[CoordinateSubset]: + return [ + CartesianPosition.default(), + CartesianVelocity.default(), + CartesianAcceleration.thrust_acceleration(), + CoordinateSubset.mass_flow_rate(), + ] + + +@pytest.fixture +def maneuver( + instants: list[Instant], + acceleration_profile: list[np.ndarray], + mass_flow_rate_profile: list[float], + frame: Frame, + coordinate_subsets: list[CoordinateSubset], +) -> Maneuver: + states = [] + for instant, acceleration, mass_flow_rate in zip( + instants, acceleration_profile, mass_flow_rate_profile + ): + states.append( + State( + instant, + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, *acceleration, mass_flow_rate], + frame, + coordinate_subsets, + ) + ) + return Maneuver(states) + + class TestConstantThrust: def test_constructors(self, guidance_law: ConstantThrust): assert guidance_law is not None @@ -89,3 +157,21 @@ def test_compute_acceleration_success( assert len(contribution) == 3 assert contribution == pytest.approx([0.0, 0.009523809523809525, 0.0], abs=5e-11) + + def test_from_maneuver(self, maneuver: Maneuver): + constant_thrust = ConstantThrust.from_maneuver( + maneuver=maneuver, + local_orbital_frame_factory=LocalOrbitalFrameFactory.TNW(Frame.GCRF()), + ) + assert isinstance(constant_thrust, ConstantThrust) + + constant_thrust_with_maximum_allowed_angular_offset = ( + ConstantThrust.from_maneuver( + maneuver=maneuver, + local_orbital_frame_factory=LocalOrbitalFrameFactory.TNW(Frame.GCRF()), + maximum_allowed_angular_offset=Angle.degrees(180.0), + ) + ) + assert isinstance( + constant_thrust_with_maximum_allowed_angular_offset, ConstantThrust + ) diff --git a/bindings/python/test/guidance_law/test_heterogeneous_guidance_law.py b/bindings/python/test/guidance_law/test_heterogeneous_guidance_law.py new file mode 100644 index 000000000..44f7ba59c --- /dev/null +++ b/bindings/python/test/guidance_law/test_heterogeneous_guidance_law.py @@ -0,0 +1,164 @@ +# Apache License 2.0 + +import numpy as np + +import pytest + +from ostk.physics.coordinate import Frame +from ostk.physics.time import Instant +from ostk.physics.time import Duration +from ostk.physics.time import Interval +from ostk.astrodynamics import GuidanceLaw +from ostk.astrodynamics.guidance_law import HeterogeneousGuidanceLaw + + +@pytest.fixture +def interval_1() -> Interval: + return Interval.closed( + start_instant=Instant.J2000(), + end_instant=Instant.J2000() + Duration.seconds(100.0), + ) + + +@pytest.fixture +def guidance_law_1() -> GuidanceLaw: + class GuidanceLaw1(GuidanceLaw): + + def __init__( + self, + name: str, + ): + super().__init__(name) + + def calculate_thrust_acceleration_at( + self, + instant: Instant, + position_coordinates: np.array, + velocity_coordinates: np.array, + thrust_acceleration: float, + output_frame: Frame, + ) -> np.array: + return np.array([1.0, 2.0, 3.0]) + + return GuidanceLaw1("My Guidance Law 1") + + +@pytest.fixture +def interval_2() -> Interval: + return Interval.closed( + start_instant=Instant.J2000() + Duration.seconds(200.0), + end_instant=Instant.J2000() + Duration.seconds(300.0), + ) + + +@pytest.fixture +def guidance_law_2() -> GuidanceLaw: + class GuidanceLaw2(GuidanceLaw): + + def __init__( + self, + name: str, + ): + super().__init__(name) + + def calculate_thrust_acceleration_at( + self, + instant: Instant, + position_coordinates: np.array, + velocity_coordinates: np.array, + thrust_acceleration: float, + output_frame: Frame, + ) -> np.array: + return np.array([4.0, 5.0, 6.0]) + + return GuidanceLaw2("My Guidance Law 2") + + +@pytest.fixture +def heterogeneous_guidance_law( + interval_1: Interval, + guidance_law_1: GuidanceLaw, + interval_2: Interval, + guidance_law_2: GuidanceLaw, +) -> HeterogeneousGuidanceLaw: + return HeterogeneousGuidanceLaw( + guidance_laws_with_intervals=[ + (guidance_law_1, interval_1), + (guidance_law_2, interval_2), + ], + ) + + +class TestHeterogeneousGuidanceLaw: + def test_constructor_and_getters( + self, + interval_1: Interval, + guidance_law_1: GuidanceLaw, + interval_2: Interval, + guidance_law_2: GuidanceLaw, + ): + heterogeneous_guidance_law = HeterogeneousGuidanceLaw( + guidance_laws_with_intervals=[ + (guidance_law_1, interval_1), + (guidance_law_2, interval_2), + ], + ) + assert heterogeneous_guidance_law is not None + assert isinstance(heterogeneous_guidance_law, HeterogeneousGuidanceLaw) + assert heterogeneous_guidance_law.get_guidance_laws_with_intervals() == [ + (guidance_law_1, interval_1), + (guidance_law_2, interval_2), + ] + + def test_add_guidance_law( + self, + interval_1: Interval, + guidance_law_1: GuidanceLaw, + interval_2: Interval, + guidance_law_2: GuidanceLaw, + ): + heterogeneous_guidance_law = HeterogeneousGuidanceLaw() + + assert isinstance(heterogeneous_guidance_law, HeterogeneousGuidanceLaw) + assert heterogeneous_guidance_law.get_guidance_laws_with_intervals() == [] + + heterogeneous_guidance_law.add_guidance_law(guidance_law_1, interval_1) + assert heterogeneous_guidance_law.get_guidance_laws_with_intervals() == [ + (guidance_law_1, interval_1), + ] + + heterogeneous_guidance_law.add_guidance_law(guidance_law_2, interval_2) + assert heterogeneous_guidance_law.get_guidance_laws_with_intervals() == [ + (guidance_law_1, interval_1), + (guidance_law_2, interval_2), + ] + + @pytest.mark.parametrize( + ( + "instant", + "expected_thrust_acceleration", + ), + [ + (Instant.J2000() - Duration.seconds(50.0), np.array([0.0, 0.0, 0.0])), + (Instant.J2000() + Duration.seconds(50.0), np.array([1.0, 2.0, 3.0])), + (Instant.J2000() + Duration.seconds(150.0), np.array([0.0, 0.0, 0.0])), + (Instant.J2000() + Duration.seconds(250.0), np.array([4.0, 5.0, 6.0])), + (Instant.J2000() + Duration.seconds(350.0), np.array([0.0, 0.0, 0.0])), + ], + ) + def test_calculate_thrust_acceleration_at( + self, + heterogeneous_guidance_law: HeterogeneousGuidanceLaw, + instant: Instant, + expected_thrust_acceleration: np.array, + ): + assert np.array_equal( + heterogeneous_guidance_law.calculate_thrust_acceleration_at( + instant=instant, + position_coordinates=np.array([0.0, 0.0, 0.0]), + velocity_coordinates=np.array([0.0, 0.0, 0.0]), + thrust_acceleration=1.0, + output_frame=Frame.GCRF(), + ), + expected_thrust_acceleration, + ) diff --git a/bindings/python/test/trajectory/test_segment.py b/bindings/python/test/trajectory/test_segment.py index 92999ae7b..4c4a24bb8 100644 --- a/bindings/python/test/trajectory/test_segment.py +++ b/bindings/python/test/trajectory/test_segment.py @@ -10,6 +10,7 @@ from ostk.physics.time import Duration from ostk.physics.coordinate import Frame from ostk.physics.environment.object.celestial import Earth +from ostk.physics.unit import Angle from ostk.astrodynamics.flight.system import SatelliteSystem from ostk.astrodynamics import Dynamics @@ -17,6 +18,7 @@ from ostk.astrodynamics.dynamics import PositionDerivative from ostk.astrodynamics.dynamics import Thruster from ostk.astrodynamics.guidance_law import ConstantThrust +from ostk.astrodynamics.trajectory import LocalOrbitalFrameFactory from ostk.astrodynamics.trajectory import State from ostk.astrodynamics.trajectory import Segment from ostk.astrodynamics.event_condition import InstantCondition @@ -75,6 +77,11 @@ def dynamics( ] +@pytest.fixture +def local_orbital_frame_factory() -> LocalOrbitalFrameFactory: + return LocalOrbitalFrameFactory.VNC(Frame.GCRF()) + + @pytest.fixture def numerical_solver() -> NumericalSolver: return NumericalSolver.default_conditional() @@ -122,6 +129,51 @@ def maneuver_segment( ) +@pytest.fixture +def maximum_allowed_angular_offset() -> Angle: + return Angle.degrees(180.0) + + +@pytest.fixture +def constant_local_orbital_frame_direction_maneuver_segment( + name: str, + instant_condition: InstantCondition, + thruster_dynamics: Thruster, + dynamics: list, + numerical_solver: NumericalSolver, + local_orbital_frame_factory: LocalOrbitalFrameFactory, +) -> Segment: + return Segment.constant_local_orbital_frame_direction_maneuver( + name=name, + event_condition=instant_condition, + thruster_dynamics=thruster_dynamics, + dynamics=dynamics, + numerical_solver=numerical_solver, + local_orbital_frame_factory=local_orbital_frame_factory, + ) + + +@pytest.fixture +def constant_local_orbital_frame_direction_maneuver_segment_with_maximum_allowed_angular_offset( + name: str, + instant_condition: InstantCondition, + thruster_dynamics: Thruster, + dynamics: list, + numerical_solver: NumericalSolver, + local_orbital_frame_factory: LocalOrbitalFrameFactory, + maximum_allowed_angular_offset: Angle, +) -> Segment: + return Segment.constant_local_orbital_frame_direction_maneuver( + name=name, + event_condition=instant_condition, + thruster_dynamics=thruster_dynamics, + dynamics=dynamics, + numerical_solver=numerical_solver, + local_orbital_frame_factory=local_orbital_frame_factory, + maximum_allowed_angular_offset=maximum_allowed_angular_offset, + ) + + @pytest.fixture def instants(state: State) -> list[Instant]: return [state.get_instant(), state.get_instant() + Duration.minutes(1.0)] @@ -293,6 +345,41 @@ def test_maneuver( is not None ) + def test_constant_local_orbital_frame_direction_maneuver( + self, + name: str, + instant_condition: InstantCondition, + thruster_dynamics: ConstantThrust, + dynamics: list, + numerical_solver: NumericalSolver, + local_orbital_frame_factory: LocalOrbitalFrameFactory, + maximum_allowed_angular_offset: Angle, + ): + assert ( + Segment.constant_local_orbital_frame_direction_maneuver( + name=name, + event_condition=instant_condition, + thruster_dynamics=thruster_dynamics, + dynamics=dynamics, + numerical_solver=numerical_solver, + local_orbital_frame_factory=local_orbital_frame_factory, + ) + is not None + ) + + assert ( + Segment.constant_local_orbital_frame_direction_maneuver( + name=name, + event_condition=instant_condition, + thruster_dynamics=thruster_dynamics, + dynamics=dynamics, + numerical_solver=numerical_solver, + local_orbital_frame_factory=local_orbital_frame_factory, + maximum_allowed_angular_offset=maximum_allowed_angular_offset, + ) + is not None + ) + def test_create_solution( self, dynamics: list, @@ -310,7 +397,7 @@ def test_create_solution( assert segment_solution is not None - def test_solve( + def test_solve_maneuver_segment( self, state: State, end_instant: Instant, @@ -318,7 +405,7 @@ def test_solve( instants: list[Instant], numerical_solver: NumericalSolver, ): - solution = maneuver_segment.solve(state) + solution: Segment.Solution = maneuver_segment.solve(state) assert ( pytest.approx( @@ -400,3 +487,21 @@ def test_solve( solution.get_dynamics_acceleration_contribution( solution.dynamics[0], state.get_frame() ) + + def test_solve_constant_local_orbital_frame_direction_maneuver_segment( + self, + state: State, + constant_local_orbital_frame_direction_maneuver_segment: Segment, + constant_local_orbital_frame_direction_maneuver_segment_with_maximum_allowed_angular_offset: Segment, + ): + solution: Segment.Solution = ( + constant_local_orbital_frame_direction_maneuver_segment.solve(state) + ) + assert solution is not None + + solution_with_maximum_allowed_angular_offset: Segment.Solution = ( + constant_local_orbital_frame_direction_maneuver_segment_with_maximum_allowed_angular_offset.solve( + state + ) + ) + assert solution_with_maximum_allowed_angular_offset is not None diff --git a/include/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.hpp b/include/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.hpp index 276d4155f..e7d1e191a 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.hpp @@ -92,8 +92,6 @@ class Thruster : public Dynamics private: const SatelliteSystem satelliteSystem_; const Shared guidanceLaw_; - const String name_; - const Real massFlowRateCache_; }; diff --git a/include/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.hpp b/include/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.hpp index 21f54743e..f23e24825 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.hpp @@ -17,9 +17,12 @@ #include #include #include +#include #include #include +#include +#include #include namespace ostk @@ -45,9 +48,12 @@ using ostk::physics::coordinate::Frame; using ostk::physics::time::Duration; using ostk::physics::time::Instant; using ostk::physics::time::Interval; +using ostk::physics::unit::Angle; using ostk::physics::unit::Mass; using ostk::astrodynamics::dynamics::Tabulated; +using ostk::astrodynamics::trajectory::LocalOrbitalFrameDirection; +using ostk::astrodynamics::trajectory::LocalOrbitalFrameFactory; using ostk::astrodynamics::trajectory::state::CoordinateSubset; #define DEFAULT_MANEUVER_INTERPOLATION_TYPE Interpolator::Type::BarycentricRational @@ -62,6 +68,8 @@ class Maneuver static const Duration MaximumRecommendedInterpolationInterval; static const Array> RequiredCoordinateSubsets; + typedef Pair MeanDirectionAndMaximumAngularOffset; + /// @brief Constructor /// /// @code{.cpp} @@ -176,6 +184,42 @@ class Maneuver const Interpolator::Type& anInterpolationType = DEFAULT_MANEUVER_INTERPOLATION_TYPE ) const; + /// @brief Calculate the mean thrust direction in the Local Orbital Frame and its maximum angular offset w.r.t. the + /// maneuver's thrust acceleration directions. + /// + /// @param aLocalOrbitalFrameFactorySPtr A local orbital frame factory + /// + /// @return The mean thrust direction and its maximum angular offset + Maneuver::MeanDirectionAndMaximumAngularOffset calculateMeanThrustDirectionAndMaximumAngularOffset( + const Shared& aLocalOrbitalFrameFactorySPtr + ) const; + + /// @brief Create a new maneuver with a constant thrust acceleration direction in the Local Orbital + /// Frame. + /// + /// The new Maneuver contains the same states as the original Maneuver, but the thrust acceleration direction is + /// constant in the Local Orbital Frame. Said direction is the mean direction of the thrust acceleration directions + /// in the Local Orbital Frame of the original Maneuver. The thrust acceleration magnitude profile is the same as + /// the original. + /// + /// If defined, a runtime error will be thrown if the maximum allowed angular offset between the original thrust + /// acceleration direction and the mean thrust direction is violated. + /// + /// @code{.cpp} + /// Maneuver newManeuver = + /// maneuver.toConstantLocalOrbitalFrameDirectionManeuver(LocalOrbitalFrameFactory::TNW(Frame::GCRF())); + /// @endcode + /// + /// @param aLocalOrbitalFrameFactorySPtr A local orbital frame factory + /// @param aMaximumAllowedAngularOffset A maximum allowed angular offset to consider (if any). Defaults to + /// Undefined. + /// + /// @return A new maneuver + Maneuver toConstantLocalOrbitalFrameDirectionManeuver( + const Shared& aLocalOrbitalFrameFactorySPtr, + const Angle& aMaximumAllowedAngularOffset = Angle::Undefined() + ) const; + /// @brief Print Maneuver /// /// @param anOutputStream An output stream diff --git a/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.hpp b/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.hpp index 0edde02c3..25dd949dd 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.hpp @@ -8,9 +8,12 @@ #include #include +#include #include +#include #include +#include namespace ostk { @@ -24,8 +27,11 @@ using ostk::core::type::Real; using ostk::mathematics::object::Vector3d; using ostk::physics::time::Instant; +using ostk::physics::unit::Angle; +using ostk::astrodynamics::flight::Maneuver; using ostk::astrodynamics::trajectory::LocalOrbitalFrameDirection; +using ostk::astrodynamics::trajectory::LocalOrbitalFrameFactory; /// @brief Define the acceleration experienced by a point mass due to a constant thrust /// guidance law @@ -47,8 +53,8 @@ class ConstantThrust : public GuidanceLaw /// @brief Output stream operator /// - /// @param in] anOutputStream An output stream - /// @param aThruster A constant thrust thruster dynamics + /// @param anOutputStream An output stream + /// @param aConstantThrust A constant thrust guidance law /// @return A reference to output stream friend std::ostream& operator<<(std::ostream& anOutputStream, const ConstantThrust& aConstantThrust); @@ -56,7 +62,7 @@ class ConstantThrust : public GuidanceLaw /// /// @code{.cpp} /// LocalOrbitalFrameDirection localThrustDirection = - /// constantThrustThruster.getLocalThrustDirection(); + /// constantThrustGuidanceLaw.getLocalThrustDirection(); /// @endcode /// /// @return Local orbital frame direction @@ -79,21 +85,46 @@ class ConstantThrust : public GuidanceLaw const Shared& outputFrameSPtr ) const override; - /// @brief Print constant thrust thruster dynamics + /// @brief Print constant thrust guidance law /// /// @param anOutputStream An output stream /// @param displayDecorator A boolean indicating whether or not to display the decorator /// during printing virtual void print(std::ostream& anOutputStream, bool displayDecorator = true) const override; - /// @brief Intrack Constant thrust dynamics + /// @brief Intrack Constant thrust guidance law /// /// @param velocityDirection A bool representing the direction of the thrust, with true /// meaning along the velocity direction. Defaults to true. /// - /// @return Constant Thrust dynamics + /// @return Constant Thrust guidance law static ConstantThrust Intrack(const bool& velocityDirection = true); + /// @brief Create a constant thrust guidance law from a maneuver. + /// + /// The local orbital frame maneuver's mean thrust direction is calculated and used to create a + /// constant thrust guidance law in said direction. + /// + /// If defined, a runtime error will be thrown if the maximum allowed angular offset between the original thrust + /// acceleration direction and the mean thrust direction is violated. + /// + /// @code{.cpp} + /// ConstantThrust constantThrustGuidanceLaw = ConstantThrust::FromManeuver(maneuver, + /// LocalOrbitalFrameFactory::TNW(Frame::GCRF())); + /// @endcode + /// + /// @param aManeuver A maneuver + /// @param aLocalOrbitalFrameFactorySPtr A local orbital frame factory + /// @param aMaximumAllowedAngularOffset A maximum allowed angular offset to consider (if any). Defaults to + /// Undefined. + /// + /// @return Constant Thrust guidance law + static ConstantThrust FromManeuver( + const Maneuver& aManeuver, + const Shared& aLocalOrbitalFrameFactorySPtr, + const Angle& aMaximumAllowedAngularOffset = Angle::Undefined() + ); + private: LocalOrbitalFrameDirection localOrbitalFrameDirection_; }; diff --git a/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.hpp b/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.hpp new file mode 100644 index 000000000..59d16a56e --- /dev/null +++ b/include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.hpp @@ -0,0 +1,90 @@ +/// Apache License 2.0 + +#ifndef __OpenSpaceToolkit_Astrodynamics_GuidanceLaw_HeterogeneousGuidanceLaw__ +#define __OpenSpaceToolkit_Astrodynamics_GuidanceLaw_HeterogeneousGuidanceLaw__ + +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace ostk +{ +namespace astrodynamics +{ +namespace guidancelaw +{ + +using ostk::core::container::Array; +using ostk::core::container::Pair; +using ostk::core::type::Real; +using ostk::core::type::Shared; + +using ostk::mathematics::object::Vector3d; + +using ostk::physics::coordinate::Frame; +using ostk::physics::time::Instant; +using ostk::physics::time::Interval; + +/// @brief The Heterogeneous Guidance Law encompasses a sequence of one or more guidance laws to be used over +/// specific time intervals. +/// At each point in time, the applicable guidance law is selected and used to calculate the thrust acceleration. +/// Guidance laws don't need to be contiguous, and can be added in any order. If the instant does not fall within any +/// of the intervals, the thrust acceleration is zero. The guidance law intervals must not intersect each other. +class HeterogeneousGuidanceLaw : public GuidanceLaw +{ + public: + /// @brief Default constructor + /// + /// @param aGuidanceLawWithIntervalArray Array of tuples containing the guidance law and their corresponding + /// interval. Defaults to empty. + HeterogeneousGuidanceLaw( + const Array, Interval>>& aGuidanceLawWithIntervalArray = + Array, Interval>>::Empty() + ); + + /// @brief Get guidance laws with their corresponding intervals + /// + /// @return Array of tuples containing the guidance laws and their corresponding intervals + Array, Interval>> getGuidanceLawsWithIntervals() const; + + /// @brief Add a guidance law with its corresponding interval + /// + /// @param aGuidanceLawSPtr The guidance law to add + /// @param anInterval The interval during which the guidance law is active + void addGuidanceLaw(const Shared& aGuidanceLawSPtr, const Interval& anInterval); + + /// @brief Calculate thrust acceleration at a given instant and state + /// + /// @param anInstant An instant + /// @param aPositionCoordinates The position coordinates + /// @param aVelocityCoordinates The velocity coordinates + /// @param aThrustAcceleration The thrust acceleration + /// @param outputFrameSPtr The frame in which the acceleration is expressed + /// + /// @return The acceleration at the provided coordinates + virtual Vector3d calculateThrustAccelerationAt( + const Instant& anInstant, + const Vector3d& aPositionCoordinates, + const Vector3d& aVelocityCoordinates, + const Real& aThrustAcceleration, + const Shared& outputFrameSPtr + ) const override; + + private: + Array intervals_; + Array> guidanceLaws_; +}; + +} // namespace guidancelaw +} // namespace astrodynamics +} // namespace ostk + +#endif diff --git a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.hpp b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.hpp index ca5f15678..e52f4cbe1 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.hpp @@ -14,12 +14,14 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include @@ -41,12 +43,14 @@ using ostk::mathematics::object::MatrixXd; using ostk::physics::time::Duration; using ostk::physics::time::Instant; using ostk::physics::time::Interval; +using ostk::physics::unit::Angle; using ostk::physics::unit::Mass; using ostk::astrodynamics::Dynamics; using ostk::astrodynamics::dynamics::Thruster; using ostk::astrodynamics::EventCondition; using flightManeuver = ostk::astrodynamics::flight::Maneuver; +using ostk::astrodynamics::trajectory::LocalOrbitalFrameFactory; using ostk::astrodynamics::trajectory::State; using ostk::astrodynamics::trajectory::state::NumericalSolver; @@ -258,13 +262,50 @@ class Segment const NumericalSolver& aNumericalSolver ); + /// @brief Create a maneuvering segment that produces maneuvers with a constant direction in the local orbital + /// frame. + /// + /// The provided thruster dynamics are used to solve the segment at first. The maneuvers produced by this segement + /// solution are then used to create a new thruster dynamics with a constant direction in the local orbital frame. + /// This new thruster dynamics is then used to actually solve the segment. + /// + /// If defined, a runtime error will be thrown if the maximum allowed angular offset between the original thruster + /// dynamics and the mean thrust direction is violated. + /// + /// @param aName A name + /// @param anEventConditionSPtr An event condition + /// @param aThrusterDynamics Dynamics for the thruster + /// @param aDynamicsArray Array of dynamics + /// @param aNumericalSolver Numerical solver + /// @param aLocalOrbitalFrameFactory A local orbital frame factory. + /// @param aMaximumAllowedAngularOffset A maximum allowed angular offset to consider (if any). Defaults + /// to Undefined. + static Segment ConstantLocalOrbitalFrameDirectionManeuver( + const String& aName, + const Shared& anEventConditionSPtr, + const Shared& aThrusterDynamics, + const Array>& aDynamicsArray, + const NumericalSolver& aNumericalSolver, + const Shared& aLocalOrbitalFrameFactory, + const Angle& aMaximumAllowedAngularOffset = Angle::Undefined() + ); + private: String name_; Type type_; Shared eventCondition_; Array> dynamics_; NumericalSolver numericalSolver_; + Shared constantManeuverDirectionLocalOrbitalFrameFactory_; + Angle constantManeuverDirectionMaximumAllowedAngularOffset_; + /// @brief Constructor + /// + /// @param aName The name of the segment + /// @param aType The type of the segment + /// @param anEventConditionSPtr The event condition + /// @param aDynamicsArray The dynamics array + /// @param aNumericalSolver The numerical solver Segment( const String& aName, const Type& aType, @@ -272,6 +313,27 @@ class Segment const Array>& aDynamicsArray, const NumericalSolver& aNumericalSolver ); + + /// @brief Solve the segment using the given dynamics and event condition + /// + /// @param aState The initial state of the segment + /// @param maximumPropagationDuration The maximum propagation duration + /// @param aDynamicsArray The dynamics array + /// @param anEventConditionSPtr The event condition + /// @return The segment solution + Segment::Solution Solve_( + const State& aState, + const Duration& maximumPropagationDuration, + const Array>& aDynamicsArray, + const Shared& anEventConditionSPtr + ) const; + + /// @brief Find the Thruster dynamics from an array of dynamics, throwing an error if none or multiple Thruster + /// dynamics are found. + /// + /// @param aDynamicsArray Array of dynamics to search. + /// @return Shared pointer to the Thruster dynamics. + static const Shared FindThrusterDynamics(const Array>& aDynamicsArray); }; } // namespace trajectory diff --git a/src/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.cpp b/src/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.cpp index 4a6a1e798..abd1334e6 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.cpp @@ -112,7 +112,7 @@ void Thruster::print(std::ostream& anOutputStream, bool displayDecorator) const { displayDecorator ? ostk::core::utils::Print::Header(anOutputStream, "Thruster") : void(); - ostk::core::utils::Print::Line(anOutputStream) << "Name:" << name_; + ostk::core::utils::Print::Line(anOutputStream) << "Name:" << this->getName(); satelliteSystem_.print(anOutputStream, false); guidanceLaw_->print(anOutputStream, false); diff --git a/src/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.cpp b/src/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.cpp index 881a26d10..c320ea022 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.cpp @@ -5,12 +5,15 @@ #include #include +#include #include #include +#include #include #include #include +#include namespace ostk { @@ -19,11 +22,13 @@ namespace astrodynamics namespace flight { +using ostk::physics::coordinate::Transform; using EarthGravitationalModel = ostk::physics::environment::gravitational::Earth; using ostk::astrodynamics::trajectory::state::coordinatesubset::CartesianAcceleration; using ostk::astrodynamics::trajectory::state::coordinatesubset::CartesianPosition; using ostk::astrodynamics::trajectory::state::coordinatesubset::CartesianVelocity; +using ostk::astrodynamics::trajectory::StateBuilder; const Shared Maneuver::DefaultAccelFrameSPtr = Frame::GCRF(); const Duration Maneuver::MinimumRecommendedDuration = Duration::Seconds(30.0); @@ -266,6 +271,101 @@ Shared Maneuver::toTabulatedDynamics( ); } +Maneuver::MeanDirectionAndMaximumAngularOffset Maneuver::calculateMeanThrustDirectionAndMaximumAngularOffset( + const Shared& aLocalOrbitalFrameFactorySPtr +) const +{ + if ((aLocalOrbitalFrameFactorySPtr == nullptr) || !aLocalOrbitalFrameFactorySPtr->isDefined()) + { + throw ostk::core::error::runtime::Undefined("Local Orbital Frame Factory"); + } + + Array thrustAccelerationsInLof = Array::Empty(); + thrustAccelerationsInLof.reserve(states_.getSize()); + Vector3d sumInLof = Vector3d::Zero(); + Angle maximumAngularOffset = Angle::Zero(); + + for (const auto& state : states_) + { + const Shared& lofFrame = aLocalOrbitalFrameFactorySPtr->generateFrame(state); + + const Vector3d thrustAcceleration = state.extractCoordinate(DefaultAccelerationCoordinateSubsetSPtr); + const Transform transform = state.accessFrame()->getTransformTo(lofFrame, state.accessInstant()); + const Vector3d thrustAccelerationInLof = transform.applyToVector(thrustAcceleration); + thrustAccelerationsInLof.add(thrustAccelerationInLof); + + sumInLof += thrustAccelerationInLof; + } + + Vector3d meanThrustDirectionInLof; + try + { + meanThrustDirectionInLof = sumInLof.normalized(); + } + catch (const std::exception& e) + { + throw ostk::core::error::RuntimeError( + "Error computing Maneuver's normalized mean thrust direction: [{}].", e.what() + ); + } + + for (const auto& thrustAccelerationInLof : thrustAccelerationsInLof) + { + const Angle offset = Angle::Between(thrustAccelerationInLof, meanThrustDirectionInLof); + if (offset.inDegrees(0.0, 360.0) > maximumAngularOffset.inDegrees(0.0, 360.0)) + { + maximumAngularOffset = offset; + } + } + + const LocalOrbitalFrameDirection meanLocalOrbitalFrameDirection = + LocalOrbitalFrameDirection(meanThrustDirectionInLof, aLocalOrbitalFrameFactorySPtr); + return {meanLocalOrbitalFrameDirection, maximumAngularOffset}; +} + +Maneuver Maneuver::toConstantLocalOrbitalFrameDirectionManeuver( + const Shared& aLocalOrbitalFrameFactorySPtr, + const Angle& aMaximumAllowedAngularOffset +) const +{ + const MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + this->calculateMeanThrustDirectionAndMaximumAngularOffset(aLocalOrbitalFrameFactorySPtr); + + if (aMaximumAllowedAngularOffset.isDefined() && meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0) > + aMaximumAllowedAngularOffset.inDegrees(0.0, 360.0)) + { + throw ostk::core::error::RuntimeError(String::Format( + "Maximum angular offset ({:.6f} deg) is greater than the maximum allowed ({:.6f} deg).", + meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0), + aMaximumAllowedAngularOffset.inDegrees(0.0, 360.0) + )); + } + + const Vector3d meanDirectionInLof = meanDirectionAndMaximumAngularOffset.first.getValue(); + + Array newStates = Array::Empty(); + newStates.reserve(states_.getSize()); + + for (const auto& state : states_) + { + const Instant& instant = state.accessInstant(); + const Shared& lofFrame = aLocalOrbitalFrameFactorySPtr->generateFrame(state); + const Shared& stateFrame = state.accessFrame(); + + const Real originalMagnitude = state.extractCoordinate(DefaultAccelerationCoordinateSubsetSPtr).norm(); + const Vector3d newThrustAccelerationLof = meanDirectionInLof * originalMagnitude; + const Transform transform = lofFrame->getTransformTo(stateFrame, instant); + const Vector3d newThrustAcceleration = transform.applyToVector(newThrustAccelerationLof); + + const StateBuilder fullStateBuilder = StateBuilder(stateFrame, RequiredCoordinateSubsets); + const State partialState = + State(state.accessInstant(), newThrustAcceleration, stateFrame, {DefaultAccelerationCoordinateSubsetSPtr}); + newStates.add(fullStateBuilder.expand(partialState, state)); + } + + return Maneuver(newStates); +} + void Maneuver::print(std::ostream& anOutputStream, bool displayDecorator) const { displayDecorator ? ostk::core::utils::Print::Header(anOutputStream, "Maneuver") : void(); diff --git a/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.cpp b/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.cpp index fb7551b7e..ceeb0b17e 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.cpp @@ -15,11 +15,15 @@ namespace astrodynamics namespace guidancelaw { +using ostk::core::type::Real; + using ostk::mathematics::geometry::d3::transformation::rotation::Quaternion; using ostk::physics::coordinate::Position; using ostk::physics::coordinate::Velocity; +using ostk::physics::unit::Angle; +using ostk::astrodynamics::GuidanceLaw; using ostk::astrodynamics::trajectory::LocalOrbitalFrameDirection; using ostk::astrodynamics::trajectory::LocalOrbitalFrameFactory; @@ -97,6 +101,32 @@ ConstantThrust ConstantThrust::Intrack(const bool& velocityDirection) return {localOrbitalFrameDirection}; } +ConstantThrust ConstantThrust::FromManeuver( + const Maneuver& aManeuver, + const Shared& aLocalOrbitalFrameFactorySPtr, + const Angle& aMaximumAllowedAngularOffset +) +{ + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + aManeuver.calculateMeanThrustDirectionAndMaximumAngularOffset(aLocalOrbitalFrameFactorySPtr); + + if (aMaximumAllowedAngularOffset.isDefined()) + { + const Real maximumAllowedAngularOffsetDegrees = aMaximumAllowedAngularOffset.inDegrees(0.0, 360.0); + const Real maximumAngularOffsetDegrees = meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0); + if (maximumAngularOffsetDegrees > maximumAllowedAngularOffsetDegrees) + { + throw ostk::core::error::RuntimeError(String::Format( + "Maximum angular offset ({:.6f} deg) is greater than the maximum allowed ({:.6f} deg).", + maximumAngularOffsetDegrees, + maximumAllowedAngularOffsetDegrees + )); + } + } + + return {meanDirectionAndMaximumAngularOffset.first}; +} + } // namespace guidancelaw } // namespace astrodynamics } // namespace ostk diff --git a/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.cpp b/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.cpp new file mode 100644 index 000000000..d31ee443a --- /dev/null +++ b/src/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.cpp @@ -0,0 +1,104 @@ +/// Apache License 2.0 + +#include +#include +#include + +#include + +namespace ostk +{ +namespace astrodynamics +{ +namespace guidancelaw +{ + +using ostk::core::container::Array; +using ostk::core::container::Pair; +using ostk::core::type::Real; +using ostk::core::type::Shared; +using ostk::core::type::Size; + +using ostk::mathematics::object::Vector3d; + +using ostk::physics::coordinate::Frame; +using ostk::physics::time::Instant; +using ostk::physics::time::Interval; + +using ostk::astrodynamics::GuidanceLaw; + +HeterogeneousGuidanceLaw::HeterogeneousGuidanceLaw( + const Array, Interval>>& aGuidanceLawWithIntervalArray +) + : GuidanceLaw("Heterogeneous Guidance Law"), + intervals_(Array::Empty()), + guidanceLaws_(Array>::Empty()) +{ + for (const Pair, Interval>& guidanceLawWithInterval : aGuidanceLawWithIntervalArray) + { + this->addGuidanceLaw(guidanceLawWithInterval.first, guidanceLawWithInterval.second); + } +} + +Array, Interval>> HeterogeneousGuidanceLaw::getGuidanceLawsWithIntervals() const +{ + Array, Interval>> guidanceLawsWithIntervals = + Array, Interval>>::Empty(); + for (Size i = 0; i < intervals_.getSize(); ++i) + { + guidanceLawsWithIntervals.add(Pair, Interval>(guidanceLaws_[i], intervals_[i])); + } + return guidanceLawsWithIntervals; +} + +void HeterogeneousGuidanceLaw::addGuidanceLaw(const Shared& aGuidanceLawSPtr, const Interval& anInterval) +{ + if (aGuidanceLawSPtr == nullptr) + { + throw ostk::core::error::RuntimeError("Guidance law cannot be null."); + } + + if (!anInterval.isDefined()) + { + throw ostk::core::error::runtime::Undefined("Interval"); + } + + for (const Interval& interval : intervals_) + { + if (interval.intersects(anInterval)) + { + throw ostk::core::error::RuntimeError(ostk::core::type::String::Format( + "Interval intersects: {} and {}", interval.toString(), anInterval.toString() + )); + } + } + + intervals_.add(anInterval); + guidanceLaws_.add(aGuidanceLawSPtr); +} + +Vector3d HeterogeneousGuidanceLaw::calculateThrustAccelerationAt( + const Instant& anInstant, + const Vector3d& aPositionCoordinates, + const Vector3d& aVelocityCoordinates, + const Real& aThrustAcceleration, + const Shared& outputFrameSPtr +) const +{ + // Check each interval to find which guidance law to use + for (Size i = 0; i < intervals_.getSize(); ++i) + { + if (intervals_[i].contains(anInstant)) + { + return guidanceLaws_[i]->calculateThrustAccelerationAt( + anInstant, aPositionCoordinates, aVelocityCoordinates, aThrustAcceleration, outputFrameSPtr + ); + } + } + + return Vector3d {0.0, 0.0, 0.0}; +} + +} // namespace guidancelaw +} // namespace astrodynamics +} // namespace ostk diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.cpp index 983d0be46..33aaa4f20 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.cpp @@ -5,6 +5,8 @@ #include #include +#include +#include #include #include #include @@ -24,6 +26,8 @@ namespace trajectory using EarthGravitationalModel = ostk::physics::environment::gravitational::Earth; using TabulatedDynamics = ostk::astrodynamics::dynamics::Tabulated; +using ostk::astrodynamics::guidancelaw::ConstantThrust; +using ostk::astrodynamics::guidancelaw::HeterogeneousGuidanceLaw; using ostk::astrodynamics::trajectory::orbit::model::Propagated; using ostk::astrodynamics::trajectory::Propagator; using ostk::astrodynamics::trajectory::state::CoordinateSubset; @@ -126,6 +130,11 @@ Mass Segment::Solution::computeDeltaMass() const Array Segment::Solution::extractManeuvers(const Shared& aFrameSPtr) const { + if ((aFrameSPtr == nullptr) || (!aFrameSPtr->isDefined())) + { + throw ostk::core::error::runtime::Undefined("Frame"); + } + if (this->states.isEmpty()) { throw ostk::core::error::RuntimeError("No states exist within Segment Solution."); @@ -136,22 +145,7 @@ Array Segment::Solution::extractManeuvers(const Shared thrusterDynamics = nullptr; - for (const Shared& dynamic : this->dynamics) - { - thrusterDynamics = std::dynamic_pointer_cast(dynamic); - - if (thrusterDynamics) - { - break; - } - } - - if (thrusterDynamics == nullptr) - { - throw ostk::core::error::RuntimeError("No Thruster dynamics found in Maneuvering segment."); - } + const Shared thrusterDynamics = FindThrusterDynamics(this->dynamics); const MatrixXd fullSegmentContributions = this->getDynamicsContribution( thrusterDynamics, aFrameSPtr, {CartesianVelocity::Default(), CoordinateSubset::Mass()} @@ -375,8 +369,6 @@ MatrixXd Segment::Solution::getDynamicsAccelerationContribution( Map, MatrixXd> Segment::Solution::getAllDynamicsContributions(const Shared& aFrameSPtr ) const { - // TBI: Use smart caching for multiple calls in the future - // Each MatrixXd contains the contribution of a single dynamics for all the segment states Map, MatrixXd> dynamicsContributionsMap = Map, MatrixXd>(); @@ -438,7 +430,9 @@ Segment::Segment( type_(aType), eventCondition_(anEventConditionSPtr), dynamics_(aDynamicsArray), - numericalSolver_(aNumericalSolver) + numericalSolver_(aNumericalSolver), + constantManeuverDirectionLocalOrbitalFrameFactory_(nullptr), + constantManeuverDirectionMaximumAllowedAngularOffset_(Angle::Undefined()) { if (eventCondition_ == nullptr) { @@ -505,33 +499,55 @@ const NumericalSolver& Segment::accessNumericalSolver() const Segment::Solution Segment::solve(const State& aState, const Duration& maximumPropagationDuration) const { - const Propagator propagator = { - numericalSolver_, - dynamics_, - }; + const Segment::Solution solution = this->Solve_(aState, maximumPropagationDuration, dynamics_, eventCondition_); - const NumericalSolver::ConditionSolution conditionSolution = propagator.calculateStateToCondition( - aState, aState.accessInstant() + maximumPropagationDuration, *eventCondition_ - ); + // If we're not forcing a constant maneuver direction in the Local Orbital Frame, return the solution + if (this->constantManeuverDirectionLocalOrbitalFrameFactory_ == nullptr || + !this->constantManeuverDirectionLocalOrbitalFrameFactory_->isDefined()) + { + return solution; + } - // Expand states based on input state - const StateBuilder stateBuilder = {aState}; + const Array solutionManeuvers = solution.extractManeuvers(aState.accessFrame()); + if (solutionManeuvers.isEmpty()) + { + return solution; + } - Array states = Array::Empty(); - states.reserve(propagator.accessNumericalSolver().accessObservedStates().getSize()); + Shared heterogeneousGuidanceLaw = + std::make_shared(HeterogeneousGuidanceLaw()); - for (const State& state : propagator.accessNumericalSolver().accessObservedStates()) + // TBI: in the future, we might want to solve in-between maneuvers. + for (const flightManeuver& solutionManeuver : solutionManeuvers) { - states.add(stateBuilder.expand(state.inFrame(aState.accessFrame()), aState)); + const Shared constantThrust = std::make_shared(ConstantThrust::FromManeuver( + solutionManeuver, + this->constantManeuverDirectionLocalOrbitalFrameFactory_, + this->constantManeuverDirectionMaximumAllowedAngularOffset_ + )); + heterogeneousGuidanceLaw->addGuidanceLaw(constantThrust, solutionManeuver.getInterval()); } - return { - name_, - dynamics_, - states, - conditionSolution.conditionIsSatisfied, - type_, - }; + const Shared thrusterDynamics = FindThrusterDynamics(dynamics_); + const Shared heterogeneousThrustDynamics = std::make_shared( + thrusterDynamics->getSatelliteSystem(), + heterogeneousGuidanceLaw, + thrusterDynamics->getName() + " (With Heterogeneous Guidance Law)" + ); + + Array> dynamicsToUseWithHeterogeneousGuidanceLaw = Array>::Empty(); + dynamicsToUseWithHeterogeneousGuidanceLaw.reserve(dynamics_.getSize()); + + for (const Shared& dynamic : dynamics_) + { + if (dynamic != thrusterDynamics) + { + dynamicsToUseWithHeterogeneousGuidanceLaw.add(dynamic); + } + } + dynamicsToUseWithHeterogeneousGuidanceLaw.add(heterogeneousThrustDynamics); + + return this->Solve_(aState, maximumPropagationDuration, dynamicsToUseWithHeterogeneousGuidanceLaw, eventCondition_); } void Segment::print(std::ostream& anOutputStream, bool displayDecorator) const @@ -596,6 +612,95 @@ Segment Segment::Maneuver( }; } +Segment Segment::ConstantLocalOrbitalFrameDirectionManeuver( + const String& aName, + const Shared& anEventConditionSPtr, + const Shared& aThrusterDynamics, + const Array>& aDynamicsArray, + const NumericalSolver& aNumericalSolver, + const Shared& aLocalOrbitalFrameFactory, + const Angle& aMaximumAllowedAngularOffset +) +{ + Segment segment = { + aName, + Segment::Type::Maneuver, + anEventConditionSPtr, + aDynamicsArray + Array> {aThrusterDynamics}, + aNumericalSolver, + }; + + segment.constantManeuverDirectionLocalOrbitalFrameFactory_ = aLocalOrbitalFrameFactory; + segment.constantManeuverDirectionMaximumAllowedAngularOffset_ = aMaximumAllowedAngularOffset; + + return segment; +} + +Segment::Solution Segment::Solve_( + const State& aState, + const Duration& maximumPropagationDuration, + const Array>& aDynamicsArray, + const Shared& anEventConditionSPtr +) const +{ + const Propagator propagator = { + numericalSolver_, + aDynamicsArray, + }; + + const NumericalSolver::ConditionSolution conditionSolution = propagator.calculateStateToCondition( + aState, aState.accessInstant() + maximumPropagationDuration, *anEventConditionSPtr + ); + + // Expand states based on input state + const StateBuilder stateBuilder = {aState}; + + Array states = Array::Empty(); + states.reserve(propagator.accessNumericalSolver().accessObservedStates().getSize()); + + for (const State& state : propagator.accessNumericalSolver().accessObservedStates()) + { + states.add(stateBuilder.expand(state.inFrame(aState.accessFrame()), aState)); + } + + return { + name_, + aDynamicsArray, + states, + conditionSolution.conditionIsSatisfied, + type_, + }; +} + +const Shared Segment::FindThrusterDynamics(const Array>& aDynamicsArray) +{ + // Loop through dynamics to find Thruster dynamics + Shared thrusterDynamics = nullptr; + bool thrusterDynamicsFound = false; + for (const Shared& dynamic : aDynamicsArray) + { + const Shared candidateThruster = std::dynamic_pointer_cast(dynamic); + + if (candidateThruster) + { + if (thrusterDynamicsFound) + { + throw ostk::core::error::RuntimeError("Multiple Thruster dynamics found in Maneuvering segment."); + } + + thrusterDynamics = candidateThruster; + thrusterDynamicsFound = true; + } + } + + if (thrusterDynamics == nullptr) + { + throw ostk::core::error::RuntimeError("No Thruster dynamics found in Maneuvering segment."); + } + + return thrusterDynamics; +} + } // namespace trajectory } // namespace astrodynamics } // namespace ostk diff --git a/test/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.test.cpp index 6808671d2..8de23b8af 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Dynamics/Thruster.test.cpp @@ -50,7 +50,7 @@ class MockGuidanceLaw : public GuidanceLaw [[maybe_unused]] const Shared& outputFrameSPtr ) const override { - return {0.0, 0.0, 0.0}; + return {1.0, 2.0, 3.0}; } }; @@ -61,7 +61,6 @@ class OpenSpaceToolkit_Astrodynamics_Dynamics_Thruster : public ::testing::Test std::make_shared(MockGuidanceLaw("test guidance law")); const SatelliteSystem defaultSatelliteSystem_ = SatelliteSystem::Default(); const String defaultName_ = "TestThruster"; - const Thruster defaultThruster_ = {defaultSatelliteSystem_, defaultGuidanceLaw_, defaultName_}; }; @@ -156,9 +155,9 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Dynamics_Thruster, ComputeContribution) const VectorXd acceleration = defaultThruster_.computeContribution(Instant::J2000(), coordinates, Frame::GCRF()); - VectorXd expectedAcceleration(4); - expectedAcceleration << 0.0, 0.0, 0.0, 0.0; - - EXPECT_TRUE(acceleration.isNear(expectedAcceleration, 1e-12)); + EXPECT_DOUBLE_EQ(acceleration[0], 1.0); + EXPECT_DOUBLE_EQ(acceleration[1], 2.0); + EXPECT_DOUBLE_EQ(acceleration[2], 3.0); + EXPECT_LT(acceleration[3], 0.0); } } diff --git a/test/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.test.cpp index a37f34a2b..f33d3037a 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.test.cpp @@ -14,12 +14,15 @@ #include #include #include +#include #include #include #include #include #include +#include +#include #include #include #include @@ -45,12 +48,15 @@ using ostk::physics::coordinate::Velocity; using ostk::physics::time::Duration; using ostk::physics::time::Instant; using ostk::physics::time::Interval; +using ostk::physics::unit::Angle; using ostk::physics::unit::Mass; using ostk::astrodynamics::Dynamics; using TabulatedDynamics = ostk::astrodynamics::dynamics::Tabulated; using ostk::astrodynamics::flight::Maneuver; using ostk::astrodynamics::flight::system::PropulsionSystem; +using ostk::astrodynamics::trajectory::LocalOrbitalFrameDirection; +using ostk::astrodynamics::trajectory::LocalOrbitalFrameFactory; using ostk::astrodynamics::trajectory::State; using ostk::astrodynamics::trajectory::state::CoordinateSubset; using ostk::astrodynamics::trajectory::state::coordinatesubset::CartesianAcceleration; @@ -61,20 +67,24 @@ class OpenSpaceToolkit_Astrodynamics_Flight_Maneuver : public ::testing::Test { protected: State stateGenerator( - const Instant& anInstant, const Vector3d& anAcceleration = {0.0, 0.0, 0.0}, const Real& aMassFlowRate = -0.5e-5 + const Instant& anInstant, + const Vector3d& anAcceleration = {0.0, 0.0, 0.0}, + const Real& aMassFlowRate = -0.5e-5, + const Vector3d& aPosition = {0.0, 0.0, 0.0}, + const Vector3d& aVelocity = {0.0, 0.0, 0.0}, + const Shared& aFrame = Frame::GCRF() ) { VectorXd coordinates(10); - coordinates << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, anAcceleration(0), anAcceleration(1), anAcceleration(2), - aMassFlowRate; + coordinates << aPosition(0), aPosition(1), aPosition(2), aVelocity(0), aVelocity(1), aVelocity(2), + anAcceleration(0), anAcceleration(1), anAcceleration(2), aMassFlowRate; - return State(anInstant, coordinates, Frame::GCRF(), Maneuver::RequiredCoordinateSubsets); + return State(anInstant, coordinates, aFrame, Maneuver::RequiredCoordinateSubsets); } const Shared defaultFrameSPtr_ = Frame::GCRF(); const Shared secondFrameSPtr_ = Frame::ITRF(); - // const Array defaultMassFlowRateProfile_ = {-1.0e-5, -1.1e-5, -0.9e-5, -1.0e-5}; const Real defaultConstantMassFlowRate_ = -0.5e-5; const Array defaultStates_ = { @@ -486,6 +496,380 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Maneuver, ToTabulatedDynamics) } } +TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Maneuver, calculateMeanThrustDirectionAndMaximumAngularOffset) +{ + const Shared inertialFrameSPtr = Frame::GCRF(); + const Shared localOrbitalFrameFactorySPtr = + LocalOrbitalFrameFactory::TNW(inertialFrameSPtr); + + { + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + defaultManeuver_.calculateMeanThrustDirectionAndMaximumAngularOffset(localOrbitalFrameFactorySPtr); + + const LocalOrbitalFrameDirection meanDirection = meanDirectionAndMaximumAngularOffset.first; + EXPECT_TRUE(meanDirection.isDefined()); + EXPECT_EQ(meanDirection.getLocalOrbitalFrameFactory(), localOrbitalFrameFactorySPtr); + + const Angle maximumAngularOffset = meanDirectionAndMaximumAngularOffset.second; + EXPECT_TRUE(maximumAngularOffset.isDefined()); + } + + { + EXPECT_THROW( + { + try + { + defaultManeuver_.calculateMeanThrustDirectionAndMaximumAngularOffset(nullptr); + } + catch (const ostk::core::error::runtime::Undefined& e) + { + EXPECT_EQ("{Local Orbital Frame Factory} is undefined.", e.getMessage()); + throw; + } + }, + ostk::core::error::runtime::Undefined + ); + } + + { + EXPECT_THROW( + { + try + { + defaultManeuver_.calculateMeanThrustDirectionAndMaximumAngularOffset( + LocalOrbitalFrameFactory::Undefined() + ); + } + catch (const ostk::core::error::runtime::Undefined& e) + { + EXPECT_EQ("{Local Orbital Frame Factory} is undefined.", e.getMessage()); + throw; + } + }, + ostk::core::error::runtime::Undefined + ); + } + + { + const Array statesWithZeroThrustAcceleration = { + stateGenerator(Instant::J2000() + Duration::Seconds(1.0), {0.0, 0.0, 0.0}, -1.0e-5), + stateGenerator(Instant::J2000() + Duration::Seconds(5.0), {0.0, 0.0, 0.0}, -1.0e-5), + }; + const Maneuver maneuverWithZeroThrustAcceleration = {statesWithZeroThrustAcceleration}; + + EXPECT_THROW( + maneuverWithZeroThrustAcceleration.calculateMeanThrustDirectionAndMaximumAngularOffset( + localOrbitalFrameFactorySPtr + ), + ostk::core::error::RuntimeError + ); + } + + { + // Acceleration direction is constant in LoF frame ({2, -1, 3}, in TNW), with different magnitudes at each + // state: 0.8 and 1.5. + const Array originalStates = { + stateGenerator( + Instant::J2000() + Duration::Seconds(0.0), + {0.8 * 1.0, 0.8 * 2.0, 0.8 * 3.0}, // 0.8 * {2, -1, 3} in TNW + -1.0e-5, + {7.0e6, 0.0, 0.0}, + {0.0, 8.0e3, 0.0}, + Frame::GCRF() + ), + stateGenerator( + Instant::J2000() + Duration::Seconds(1.0), + {1.5 * -2.0, 1.5 * 1.0, 1.5 * 3.0}, // 1.5 * {2, -1, 3} in TNW + -1.0e-5, + {0.0, 7.0e6, 0.0}, + {-8.0e3, 0.0, 0.0}, + Frame::GCRF() + ), + }; + + const Maneuver originalManeuver = {originalStates}; + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + originalManeuver.calculateMeanThrustDirectionAndMaximumAngularOffset(localOrbitalFrameFactorySPtr); + + const LocalOrbitalFrameDirection meanDirection = meanDirectionAndMaximumAngularOffset.first; + EXPECT_TRUE(meanDirection.isDefined()); + EXPECT_EQ(meanDirection.getLocalOrbitalFrameFactory(), localOrbitalFrameFactorySPtr); + EXPECT_TRUE(meanDirection.getValue().isApprox(Vector3d(2.0, -1.0, 3.0).normalized(), 1e-12)); + + const Angle maximumAngularOffset = meanDirectionAndMaximumAngularOffset.second; + EXPECT_TRUE(maximumAngularOffset.isDefined()); + EXPECT_DOUBLE_EQ(maximumAngularOffset.inDegrees(0.0, 360.0), 0.0); + } + + { + // Acceleration direction is not constant in TNW frame, but magnitudes are. + const Array originalStates = { + stateGenerator( + Instant::J2000() + Duration::Seconds(0.0), + Vector3d {1.0, 2.0, 3.0}, // {2.0, -1.0, 3.0} in TNW + -1.0e-5, + {7.0e6, 0.0, 0.0}, + {0.0, 8.0e3, 0.0}, + Frame::GCRF() + ), + stateGenerator( + Instant::J2000() + Duration::Seconds(1.0), + Vector3d {-3.0, -1.0, -2.0}, // {3.0, 1.0, -2.0} in TNW + -1.0e-5, + {0.0, 7.0e6, 0.0}, + {-8.0e3, 0.0, 0.0}, + Frame::GCRF() + ), + }; + + const Maneuver originalManeuver = {originalStates}; + + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + originalManeuver.calculateMeanThrustDirectionAndMaximumAngularOffset(localOrbitalFrameFactorySPtr); + + const LocalOrbitalFrameDirection meanDirection = meanDirectionAndMaximumAngularOffset.first; + EXPECT_TRUE(meanDirection.isDefined()); + EXPECT_EQ(meanDirection.getLocalOrbitalFrameFactory(), localOrbitalFrameFactorySPtr); + EXPECT_TRUE(meanDirection.getValue().isApprox(Vector3d(2.5, 0.0, 0.5).normalized(), 1e-12)); + + const Angle maximumAngularOffset = meanDirectionAndMaximumAngularOffset.second; + EXPECT_TRUE(maximumAngularOffset.isDefined()); + EXPECT_DOUBLE_EQ( + maximumAngularOffset.inDegrees(0.0, 360.0), + std::max( + Angle::Between(meanDirection.getValue(), Vector3d(2.0, -1.0, 3.0)).inDegrees(0.0, 360.0), + Angle::Between(meanDirection.getValue(), Vector3d(3.0, 1.0, -2.0)).inDegrees(0.0, 360.0) + ) + ); + } + + { + // Neither direction nor magnitudes are constant. + const Array originalStates = { + stateGenerator( + Instant::J2000() + Duration::Seconds(0.0), + Vector3d {2.0, 4.0, 6.0}, // {4.0, -2.0, 6.0} in TNW + -1.0e-5, + {7.0e6, 0.0, 0.0}, + {0.0, 8.0e3, 0.0}, + Frame::GCRF() + ), + stateGenerator( + Instant::J2000() + Duration::Seconds(1.0), + Vector3d {-3.0, -1.0, -2.0}, // {3.0, 1.0, -2.0} in TNW + -1.0e-5, + {0.0, 7.0e6, 0.0}, + {-8.0e3, 0.0, 0.0}, + Frame::GCRF() + ), + }; + + const Maneuver originalManeuver = {originalStates}; + + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + originalManeuver.calculateMeanThrustDirectionAndMaximumAngularOffset(localOrbitalFrameFactorySPtr); + + const LocalOrbitalFrameDirection meanDirection = meanDirectionAndMaximumAngularOffset.first; + EXPECT_TRUE(meanDirection.isDefined()); + EXPECT_EQ(meanDirection.getLocalOrbitalFrameFactory(), localOrbitalFrameFactorySPtr); + EXPECT_TRUE(meanDirection.getValue().isApprox(Vector3d(3.5, -0.5, 2.0).normalized(), 1e-12)); + + const Angle maximumAngularOffset = meanDirectionAndMaximumAngularOffset.second; + EXPECT_TRUE(maximumAngularOffset.isDefined()); + EXPECT_DOUBLE_EQ( + maximumAngularOffset.inDegrees(0.0, 360.0), + std::max( + Angle::Between(meanDirection.getValue(), Vector3d(4.0, -2.0, 6.0)).inDegrees(0.0, 360.0), + Angle::Between(meanDirection.getValue(), Vector3d(3.0, 1.0, -2.0)).inDegrees(0.0, 360.0) + ) + ); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Maneuver, toConstantLocalOrbitalFrameDirectionManeuver) +{ + const Shared inertialFrameSPtr = Frame::GCRF(); + const Shared localOrbitalFrameFactorySPtr = + LocalOrbitalFrameFactory::TNW(inertialFrameSPtr); + + // Ignore maximum angular offset + { + const Maneuver newManeuver = + defaultManeuver_.toConstantLocalOrbitalFrameDirectionManeuver(localOrbitalFrameFactorySPtr); + + EXPECT_TRUE(newManeuver.isDefined()); + EXPECT_EQ(newManeuver.getStates().getSize(), defaultStates_.getSize()); + EXPECT_EQ(newManeuver.getInterval(), defaultManeuver_.getInterval()); + + for (Size i = 0; i < newManeuver.getStates().getSize(); i++) + { + const State originalState = defaultManeuver_.getStates()[i]; + const State newState = newManeuver.getStates()[i]; + + EXPECT_EQ(originalState.getInstant(), newState.getInstant()); + EXPECT_EQ(originalState.getFrame(), newState.getFrame()); + EXPECT_EQ(originalState.getPosition(), newState.getPosition()); + EXPECT_EQ(originalState.getVelocity(), newState.getVelocity()); + } + } + + // Consider maximum allowed angular offset, but it's not violated + { + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + defaultManeuver_.calculateMeanThrustDirectionAndMaximumAngularOffset(localOrbitalFrameFactorySPtr); + + const Maneuver newManeuver = defaultManeuver_.toConstantLocalOrbitalFrameDirectionManeuver( + localOrbitalFrameFactorySPtr, + Angle::Degrees(1.1 * meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0)) + ); + + EXPECT_TRUE(newManeuver.isDefined()); + EXPECT_EQ(newManeuver.getStates().getSize(), defaultStates_.getSize()); + EXPECT_EQ(newManeuver.getInterval(), defaultManeuver_.getInterval()); + } + + // Consider maximum allowed angular offset, and it's violated + { + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + defaultManeuver_.calculateMeanThrustDirectionAndMaximumAngularOffset(localOrbitalFrameFactorySPtr); + + EXPECT_THROW( + try { + defaultManeuver_.toConstantLocalOrbitalFrameDirectionManeuver( + localOrbitalFrameFactorySPtr, + Angle::Degrees(0.9 * meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0)) + ); + } catch (const ostk::core::error::RuntimeError& e) { + EXPECT_NE(e.getMessage().find("Maximum angular offset"), std::string::npos); + EXPECT_NE(e.getMessage().find(" is greater than the maximum allowed ("), std::string::npos); + throw; + }, + ostk::core::error::RuntimeError + ); + } + + // Local Orbital Frame Factory is undefined + { + EXPECT_THROW( + defaultManeuver_.toConstantLocalOrbitalFrameDirectionManeuver(LocalOrbitalFrameFactory::Undefined()), + ostk::core::error::runtime::Undefined + ); + } + + // Thrust acceleration direction is too small + { + const Array statesWithZeroThrustAcceleration = { + stateGenerator(Instant::J2000() + Duration::Seconds(1.0), {0.0, 0.0, 0.0}, -1.0e-5), + stateGenerator(Instant::J2000() + Duration::Seconds(5.0), {0.0, 0.0, 0.0}, -1.0e-5), + }; + + const Maneuver maneuverWithZeroThrustAcceleration = {statesWithZeroThrustAcceleration}; + EXPECT_THROW( + maneuverWithZeroThrustAcceleration.toConstantLocalOrbitalFrameDirectionManeuver(localOrbitalFrameFactorySPtr + ), + ostk::core::error::RuntimeError + ); + } + + // Acceleration direction is constant in LoF frame ({2, -1, 3}, in TNW), with different magnitudes at each + // state: 0.8 and 1.5. + { + // Acceleration direction is constant in LoF frame ({2, -1, 3}, in TNW), with different magnitudes at each + // state: 0.8 and 1.5. + const Array originalStates = { + stateGenerator( + Instant::J2000() + Duration::Seconds(0.0), + {0.8 * 1.0, 0.8 * 2.0, 0.8 * 3.0}, // 0.8 * {2, -1, 3} in TNW + -1.0e-5, + {7.0e6, 0.0, 0.0}, + {0.0, 8.0e3, 0.0}, + Frame::GCRF() + ), + stateGenerator( + Instant::J2000() + Duration::Seconds(1.0), + {1.5 * -2.0, 1.5 * 1.0, 1.5 * 3.0}, // 1.5 * {2, -1, 3} in TNW + -1.0e-5, + {0.0, 7.0e6, 0.0}, + {-8.0e3, 0.0, 0.0}, + Frame::GCRF() + ), + }; + + const Maneuver originalManeuver = {originalStates}; + const Maneuver newManeuver = + originalManeuver.toConstantLocalOrbitalFrameDirectionManeuver(localOrbitalFrameFactorySPtr); + const Array newStates = newManeuver.getStates(); + + const Vector3d expectedThrustAccelerationInLof = {2.0, -1.0, 3.0}; + + const Vector3d state0ThrustAccelerationInLof = + newStates[0] + .accessFrame() + ->getTransformTo( + localOrbitalFrameFactorySPtr->generateFrame(newStates[0]), newStates[0].accessInstant() + ) + .applyToVector(newStates[0].extractCoordinate(CartesianAcceleration::ThrustAcceleration())); + const Vector3d state1ThrustAccelerationInLof = + newStates[1] + .accessFrame() + ->getTransformTo( + localOrbitalFrameFactorySPtr->generateFrame(newStates[1]), newStates[1].accessInstant() + ) + .applyToVector(newStates[1].extractCoordinate(CartesianAcceleration::ThrustAcceleration())); + + EXPECT_TRUE(state0ThrustAccelerationInLof.isApprox(0.8 * expectedThrustAccelerationInLof, 1e-12)); + EXPECT_TRUE(state1ThrustAccelerationInLof.isApprox(1.5 * expectedThrustAccelerationInLof, 1e-12)); + } + + { + // Acceleration direction is not constant in TNW frame, but magnitudes are. + const Array originalStates = { + stateGenerator( + Instant::J2000() + Duration::Seconds(0.0), + Vector3d {1.0, 2.0, 3.0}, // {2.0, -1.0, 3.0} in TNW + -1.0e-5, + {7.0e6, 0.0, 0.0}, + {0.0, 8.0e3, 0.0}, + Frame::GCRF() + ), + stateGenerator( + Instant::J2000() + Duration::Seconds(1.0), + Vector3d {-3.0, -1.0, -2.0}, // {3.0, 1.0, -2.0} in TNW + -1.0e-5, + {0.0, 7.0e6, 0.0}, + {-8.0e3, 0.0, 0.0}, + Frame::GCRF() + ), + }; + + const Maneuver originalManeuver = {originalStates}; + const Maneuver newManeuver = + originalManeuver.toConstantLocalOrbitalFrameDirectionManeuver(localOrbitalFrameFactorySPtr); + const Array newStates = newManeuver.getStates(); + + const Vector3d expectedThrustAccelerationInLof = + Vector3d {2.5, 0.0, 0.5}.normalized() * Vector3d {1.0, 2.0, 3.0}.norm(); + + const Vector3d state0ThrustAccelerationInLof = + newStates[0] + .accessFrame() + ->getTransformTo( + localOrbitalFrameFactorySPtr->generateFrame(newStates[0]), newStates[0].accessInstant() + ) + .applyToVector(newStates[0].extractCoordinate(CartesianAcceleration::ThrustAcceleration())); + const Vector3d state1ThrustAccelerationInLof = + newStates[1] + .accessFrame() + ->getTransformTo( + localOrbitalFrameFactorySPtr->generateFrame(newStates[1]), newStates[1].accessInstant() + ) + .applyToVector(newStates[1].extractCoordinate(CartesianAcceleration::ThrustAcceleration())); + + EXPECT_TRUE(state0ThrustAccelerationInLof.isApprox(expectedThrustAccelerationInLof, 1e-12)); + EXPECT_TRUE(state1ThrustAccelerationInLof.isApprox(expectedThrustAccelerationInLof, 1e-12)); + } +} + TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Maneuver, ConstantMassFlowRateProfile) { { diff --git a/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.test.cpp index d8f2b0861..016b0e082 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/ConstantThrust.test.cpp @@ -21,17 +21,20 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include #include #include +#include #include @@ -59,6 +62,7 @@ using ostk::physics::environment::object::celestial::Earth; using ostk::physics::environment::object::celestial::Moon; using ostk::physics::environment::object::celestial::Sun; using ostk::physics::time::DateTime; +using ostk::physics::time::Duration; using ostk::physics::time::Instant; using ostk::physics::time::Scale; using ostk::physics::unit::Angle; @@ -72,6 +76,7 @@ using EarthAtmosphericModel = ostk::physics::environment::atmospheric::Earth; using ostk::astrodynamics::Dynamics; using ostk::astrodynamics::dynamics::Thruster; +using ostk::astrodynamics::flight::Maneuver; using ostk::astrodynamics::flight::system::PropulsionSystem; using ostk::astrodynamics::flight::system::SatelliteSystem; using ostk::astrodynamics::guidancelaw::ConstantThrust; @@ -79,6 +84,7 @@ using ostk::astrodynamics::trajectory::LocalOrbitalFrameDirection; using ostk::astrodynamics::trajectory::LocalOrbitalFrameFactory; using ostk::astrodynamics::trajectory::LocalOrbitalFrameTransformProvider; using ostk::astrodynamics::trajectory::state::NumericalSolver; +using ostk::astrodynamics::trajectory::StateBuilder; class OpenSpaceToolkit_Astrodynamics_GuidanceLaw_ConstantThrust : public ::testing::Test { @@ -98,9 +104,11 @@ class OpenSpaceToolkit_Astrodynamics_GuidanceLaw_ConstantThrust : public ::testi const Shared gcrfSPtr_ = Frame::GCRF(); + const Shared localOrbitalFrameFactorySPtr_ = + LocalOrbitalFrameFactory::VNC(gcrfSPtr_); LocalOrbitalFrameDirection localOrbitalFrameDirection_ = { {1.0, 0.0, 0.0}, - LocalOrbitalFrameFactory::VNC(gcrfSPtr_), + localOrbitalFrameFactorySPtr_, }; const ConstantThrust defaultConstantThrust_ = {localOrbitalFrameDirection_}; @@ -303,3 +311,76 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_GuidanceLaw_ConstantThrust, Intrack) ); } } + +TEST_F(OpenSpaceToolkit_Astrodynamics_GuidanceLaw_ConstantThrust, FromManeuver) +{ + const StateBuilder stateBuilder = StateBuilder(gcrfSPtr_, Maneuver::RequiredCoordinateSubsets); + const Maneuver maneuver = Maneuver({ + stateBuilder.build( + Instant::J2000() + Duration::Seconds(0.0), + (VectorXd(10) << 7.0e6, 0.0, 0.0, 0.0, 8.0e3, 0.0, 1.0, 2.0, 3.0, -1.0e-5).finished() + ), + stateBuilder.build( + Instant::J2000() + Duration::Seconds(10.0), + (VectorXd(10) << 0.0, 7.0e6, 0.0, -8.0e3, 0.0, 0.0, 4.0, 5.0, 6.0, -1.0e-5).finished() + ), + }); + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + maneuver.calculateMeanThrustDirectionAndMaximumAngularOffset(localOrbitalFrameFactorySPtr_); + + { + EXPECT_THROW( + try { + ConstantThrust::FromManeuver(maneuver, LocalOrbitalFrameFactory::Undefined()); + } catch (const ostk::core::error::runtime::Undefined& e) { + EXPECT_EQ("{Local Orbital Frame Factory} is undefined.", e.getMessage()); + throw; + }, + ostk::core::error::runtime::Undefined + ); + } + + // Not considering maximum allowed angular offset for now + { + ConstantThrust constantThrust = ConstantThrust::FromManeuver(maneuver, localOrbitalFrameFactorySPtr_); + EXPECT_EQ( + constantThrust.getLocalThrustDirection().getValue(), meanDirectionAndMaximumAngularOffset.first.getValue() + ); + EXPECT_EQ( + constantThrust.getLocalThrustDirection().accessLocalOrbitalFrameFactory(), localOrbitalFrameFactorySPtr_ + ); + } + + // Considering a maximum allowed angular offset, but it's not violated + { + ConstantThrust constantThrust = ConstantThrust::FromManeuver( + maneuver, + localOrbitalFrameFactorySPtr_, + Angle::Degrees(1.1 * meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0)) + ); + EXPECT_EQ( + constantThrust.getLocalThrustDirection().getValue(), meanDirectionAndMaximumAngularOffset.first.getValue() + ); + EXPECT_EQ( + constantThrust.getLocalThrustDirection().accessLocalOrbitalFrameFactory(), localOrbitalFrameFactorySPtr_ + ); + } + + // Considering a maximum allowed angular offset, and it's violated + { + EXPECT_THROW( + try { + ConstantThrust::FromManeuver( + maneuver, + localOrbitalFrameFactorySPtr_, + Angle::Degrees(0.9 * meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0)) + ); + } catch (const ostk::core::error::RuntimeError& e) { + EXPECT_NE(e.getMessage().find("Maximum angular offset"), std::string::npos); + EXPECT_NE(e.getMessage().find(" is greater than the maximum allowed ("), std::string::npos); + throw; + }, + ostk::core::error::RuntimeError + ); + } +} diff --git a/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.test.cpp new file mode 100644 index 000000000..fa299057a --- /dev/null +++ b/test/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/HeterogeneousGuidanceLaw.test.cpp @@ -0,0 +1,281 @@ +/// Apache License 2.0 + +#include + +#include +#include +#include +#include + +#include +#include + +#include + +using ostk::core::container::Array; +using ostk::core::container::Pair; +using ostk::core::type::Real; +using ostk::core::type::Shared; +using ostk::core::type::String; + +using ostk::mathematics::object::Vector3d; + +using ostk::physics::coordinate::Frame; +using ostk::physics::time::Duration; +using ostk::physics::time::Instant; +using ostk::physics::time::Interval; + +using ostk::astrodynamics::GuidanceLaw; +using ostk::astrodynamics::guidancelaw::HeterogeneousGuidanceLaw; + +class MockGuidanceLaw1 : public GuidanceLaw +{ + public: + MockGuidanceLaw1(const String& aName) + : GuidanceLaw(aName) + { + } + + Vector3d calculateThrustAccelerationAt( + [[maybe_unused]] const Instant& anInstant, + [[maybe_unused]] const Vector3d& aPositionCoordinates, + [[maybe_unused]] const Vector3d& aVelocityCoordinates, + [[maybe_unused]] const Real& aThrustDirection, + [[maybe_unused]] const Shared& outputFrameSPtr + ) const override + { + return Vector3d(1.0, 2.0, 3.0); + } +}; + +class MockGuidanceLaw2 : public GuidanceLaw +{ + public: + MockGuidanceLaw2(const String& aName) + : GuidanceLaw(aName) + { + } + + Vector3d calculateThrustAccelerationAt( + [[maybe_unused]] const Instant& anInstant, + [[maybe_unused]] const Vector3d& aPositionCoordinates, + [[maybe_unused]] const Vector3d& aVelocityCoordinates, + [[maybe_unused]] const Real& aThrustDirection, + [[maybe_unused]] const Shared& outputFrameSPtr + ) const override + { + return Vector3d(4.0, 5.0, 6.0); + } +}; + +class OpenSpaceToolkit_Astrodynamics_GuidanceLaw_HeterogeneousGuidanceLaw : public ::testing::Test +{ + protected: + const Interval interval1_ = Interval::Closed(Instant::J2000(), Instant::J2000() + Duration::Seconds(100.0)); + + const Interval interval2_ = + Interval::Closed(Instant::J2000() + Duration::Seconds(200.0), Instant::J2000() + Duration::Seconds(300.0)); + + const Shared guidanceLaw1_ = std::make_shared("My Guidance Law 1"); + const Shared guidanceLaw2_ = std::make_shared("My Guidance Law 2"); + + const HeterogeneousGuidanceLaw heterogeneousGuidanceLaw_ = + HeterogeneousGuidanceLaw(Array, Interval>>( + {Pair, Interval>(guidanceLaw1_, interval1_), + Pair, Interval>(guidanceLaw2_, interval2_)} + )); +}; + +TEST_F(OpenSpaceToolkit_Astrodynamics_GuidanceLaw_HeterogeneousGuidanceLaw, Constructor) +{ + // No arguments + { + EXPECT_NO_THROW(HeterogeneousGuidanceLaw()); + } + + // With arguments + { + EXPECT_NO_THROW(HeterogeneousGuidanceLaw(Array, Interval>>( + {Pair, Interval>(guidanceLaw1_, interval1_), + Pair, Interval>(guidanceLaw2_, interval2_)} + ))); + } + + // Undefined interval + { + EXPECT_THROW( + try { + HeterogeneousGuidanceLaw(Array, Interval>>( + {Pair, Interval>(guidanceLaw1_, Interval::Undefined())} + )); + } catch (const ostk::core::error::runtime::Undefined& e) { + EXPECT_EQ("{Interval} is undefined.", e.getMessage()); + throw; + }, + ostk::core::error::runtime::Undefined + ); + } + + // Null guidance law + { + EXPECT_THROW( + try { + HeterogeneousGuidanceLaw(Array, Interval>>( + {Pair, Interval>(std::shared_ptr(nullptr), interval1_)} + )); + } catch (const ostk::core::error::RuntimeError& e) { + EXPECT_EQ("Guidance law cannot be null.", e.getMessage()); + throw; + }, + ostk::core::error::RuntimeError + ); + } + + // Intersecting intervals + { + EXPECT_THROW( + try { + HeterogeneousGuidanceLaw(Array, Interval>>( + {Pair, Interval>(guidanceLaw1_, interval1_), + Pair, Interval>(guidanceLaw2_, interval1_)} + )); + } catch (const ostk::core::error::RuntimeError& e) { + EXPECT_NE(e.getMessage().find("Interval intersects"), std::string::npos); + throw; + }, + ostk::core::error::RuntimeError + ); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_GuidanceLaw_HeterogeneousGuidanceLaw, Getters) +{ + { + const HeterogeneousGuidanceLaw heterogeneousGuidanceLaw = + HeterogeneousGuidanceLaw(Array, Interval>>( + {Pair, Interval>(guidanceLaw1_, interval1_), + Pair, Interval>(guidanceLaw2_, interval2_)} + )); + + const auto expected = Array, Interval>>( + {Pair, Interval>(guidanceLaw1_, interval1_), + Pair, Interval>(guidanceLaw2_, interval2_)} + ); + + const auto actual = heterogeneousGuidanceLaw.getGuidanceLawsWithIntervals(); + + EXPECT_EQ(actual.getSize(), expected.getSize()); + for (size_t i = 0; i < expected.getSize(); ++i) + { + EXPECT_EQ(actual[i].first, expected[i].first); + EXPECT_EQ(actual[i].second, expected[i].second); + } + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_GuidanceLaw_HeterogeneousGuidanceLaw, AddGuidanceLaw) +{ + // Adding guidance laws one by one + { + HeterogeneousGuidanceLaw heterogeneousGuidanceLaw = HeterogeneousGuidanceLaw(); + + EXPECT_TRUE(heterogeneousGuidanceLaw.getGuidanceLawsWithIntervals().isEmpty()); + + heterogeneousGuidanceLaw.addGuidanceLaw(guidanceLaw1_, interval1_); + + auto expected = + Array, Interval>>({Pair, Interval>(guidanceLaw1_, interval1_)} + ); + auto actual = heterogeneousGuidanceLaw.getGuidanceLawsWithIntervals(); + EXPECT_EQ(actual.getSize(), expected.getSize()); + for (size_t i = 0; i < expected.getSize(); ++i) + { + EXPECT_EQ(actual[i].first, expected[i].first); + EXPECT_EQ(actual[i].second, expected[i].second); + } + + heterogeneousGuidanceLaw.addGuidanceLaw(guidanceLaw2_, interval2_); + expected = Array, Interval>>( + {Pair, Interval>(guidanceLaw1_, interval1_), + Pair, Interval>(guidanceLaw2_, interval2_)} + ); + actual = heterogeneousGuidanceLaw.getGuidanceLawsWithIntervals(); + EXPECT_EQ(actual.getSize(), expected.getSize()); + for (size_t i = 0; i < expected.getSize(); ++i) + { + EXPECT_EQ(actual[i].first, expected[i].first); + EXPECT_EQ(actual[i].second, expected[i].second); + } + } + + // Undefined interval + { + HeterogeneousGuidanceLaw heterogeneousGuidanceLaw = HeterogeneousGuidanceLaw(); + EXPECT_THROW( + try { + heterogeneousGuidanceLaw.addGuidanceLaw(guidanceLaw1_, Interval::Undefined()); + } catch (const ostk::core::error::runtime::Undefined& e) { + EXPECT_EQ("{Interval} is undefined.", e.getMessage()); + throw; + }, + ostk::core::error::runtime::Undefined + ); + } + + // Null guidance law + { + HeterogeneousGuidanceLaw heterogeneousGuidanceLaw = HeterogeneousGuidanceLaw(); + EXPECT_THROW( + try { + heterogeneousGuidanceLaw.addGuidanceLaw(std::shared_ptr(nullptr), interval1_); + } catch (const ostk::core::error::RuntimeError& e) { + EXPECT_EQ("Guidance law cannot be null.", e.getMessage()); + throw; + }, + ostk::core::error::RuntimeError + ); + } + // Intersecting intervals + { + HeterogeneousGuidanceLaw heterogeneousGuidanceLaw = HeterogeneousGuidanceLaw(); + heterogeneousGuidanceLaw.addGuidanceLaw(guidanceLaw1_, interval1_); + + EXPECT_THROW( + try { + heterogeneousGuidanceLaw.addGuidanceLaw(guidanceLaw2_, interval1_); + } catch (const ostk::core::error::RuntimeError& e) { + EXPECT_NE(e.getMessage().find("Interval intersects"), std::string::npos); + throw; + }, + ostk::core::error::RuntimeError + ); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_GuidanceLaw_HeterogeneousGuidanceLaw, CalculateThrustAccelerationAt) +{ + { + struct TestCase + { + Instant instant; + Vector3d expectedAcceleration; + }; + + const Array testCases = { + {Instant::J2000() - Duration::Seconds(50.0), Vector3d(0.0, 0.0, 0.0)}, + {Instant::J2000() + Duration::Seconds(50.0), Vector3d(1.0, 2.0, 3.0)}, + {Instant::J2000() + Duration::Seconds(150.0), Vector3d(0.0, 0.0, 0.0)}, + {Instant::J2000() + Duration::Seconds(250.0), Vector3d(4.0, 5.0, 6.0)}, + {Instant::J2000() + Duration::Seconds(350.0), Vector3d(0.0, 0.0, 0.0)} + }; + + for (const auto& testCase : testCases) + { + const Vector3d acceleration = heterogeneousGuidanceLaw_.calculateThrustAccelerationAt( + testCase.instant, Vector3d::Zero(), Vector3d::Zero(), 1.0, Frame::GCRF() + ); + + EXPECT_TRUE(acceleration.isNear(testCase.expectedAcceleration, 1e-15)); + } + } +} diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.test.cpp index c14f9fb6e..e25754fe2 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Segment.test.cpp @@ -39,10 +39,12 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -92,10 +94,12 @@ using ostk::astrodynamics::eventcondition::RealCondition; using ostk::astrodynamics::flight::Maneuver; using ostk::astrodynamics::flight::system::SatelliteSystem; using ostk::astrodynamics::guidancelaw::ConstantThrust; +using ostk::astrodynamics::guidancelaw::HeterogeneousGuidanceLaw; using ostk::astrodynamics::guidancelaw::QLaw; using ostk::astrodynamics::trajectory::LocalOrbitalFrameDirection; using ostk::astrodynamics::trajectory::LocalOrbitalFrameFactory; using ostk::astrodynamics::trajectory::orbit::model::kepler::COE; +using ostk::astrodynamics::trajectory::Propagator; using ostk::astrodynamics::trajectory::Segment; using ostk::astrodynamics::trajectory::State; using ostk::astrodynamics::trajectory::state::CoordinateBroker; @@ -149,19 +153,24 @@ class OpenSpaceToolkit_Astrodynamics_Trajectory_Segment : public ::testing::Test std::make_shared(), std::make_shared(earthSpherical_), }; + const NumericalSolver defaultNumericalSolver_ = { NumericalSolver::LogType::NoLog, NumericalSolver::StepperType::RungeKuttaDopri5, 5.0, - 1.0e-12, - 1.0e-12, + 1.0e-8, // Reducing tolerances so that the solving doesn't take forever + 1.0e-8, // Reducing tolerances so that the solving doesn't take forever }; + const Shared defaultInstantCondition_ = std::make_shared( InstantCondition::Criterion::AnyCrossing, defaultState_.accessInstant() + Duration::Minutes(15.0) ); + const Shared defaultLocalOrbitalFrameFactorySPtr_ = + LocalOrbitalFrameFactory::VNC(defaultFrameSPtr_); + const Shared constantThrustSPtr_ = std::make_shared( - LocalOrbitalFrameDirection({1.0, 0.0, 0.0}, LocalOrbitalFrameFactory::VNC(defaultFrameSPtr_)) + LocalOrbitalFrameDirection({1.0, 0.0, 0.0}, defaultLocalOrbitalFrameFactorySPtr_) ); const SatelliteSystem defaultSatelliteSystem_ = SatelliteSystem::Default(); @@ -178,6 +187,54 @@ class OpenSpaceToolkit_Astrodynamics_Trajectory_Segment : public ::testing::Test CoordinateSubset::Mass(), })); + const COE defaultCurrentCOE_ = { + Length::Meters(6800.0e3), + 0.01, + Angle::Degrees(80.0), + Angle::Degrees(0.0), + Angle::Degrees(0.0), + Angle::Degrees(90.0), + }; + + const COE defaultTargetCOE_ = { + defaultCurrentCOE_.getSemiMajorAxis(), + defaultCurrentCOE_.getEccentricity(), + defaultCurrentCOE_.getInclination() + Angle::Degrees(0.1), + defaultCurrentCOE_.getRaan(), + defaultCurrentCOE_.getAop(), + defaultCurrentCOE_.getTrueAnomaly(), + }; + + const QLaw::Parameters defaultQLawParameters_ = { + { + { + COE::Element::Inclination, + { + 1.0, // Control element weight + 1e-4, // Convergence threshold of 0.0001 + }, + }, + }, + 3, // M value + 4, // N value + 2, // R value + 0.01, // B value + 100, // K value + 1.0, // Periapsis weight + Length::Kilometers(6578.0), // Minimum periapsis + 0.5, // Absolute effectivity threshold + }; + + const QLaw defaultQLaw_ = { + defaultTargetCOE_, + EarthGravitationalModel::EGM2008.gravitationalParameter_, + defaultQLawParameters_, + QLaw::GradientStrategy::Analytical, + }; + + const Shared defaultQLawThrusterDynamicsSPtr_ = + std::make_shared(defaultSatelliteSystem_, std::make_shared(defaultQLaw_)); + State initialStateWithMass_ = State::Undefined(); State intermediateStateWithMass_ = State::Undefined(); State finalStateWithMass_ = State::Undefined(); @@ -277,25 +334,93 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Comput TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_ExtractManeuvers) { { + const Segment::Solution segmentSolution = Segment::Solution( + defaultName_, + defaultDynamics_ + Array> {defaultThrusterDynamicsSPtr_}, + {initialStateWithMass_, finalStateWithMass_}, + true, + Segment::Type::Maneuver + ); + + EXPECT_THROW( + { + try + { + segmentSolution.extractManeuvers(nullptr); + } + catch (const ostk::core::error::runtime::Undefined& e) + { + EXPECT_EQ("{Frame} is undefined.", e.getMessage()); + throw; + } + }, + ostk::core::error::runtime::Undefined + ); + EXPECT_THROW( - Segment::Solution(defaultName_, defaultDynamics_, {}, true, Segment::Type::Maneuver) - .extractManeuvers(defaultFrameSPtr_), + { + try + { + segmentSolution.extractManeuvers(Frame::Undefined()); + } + catch (const ostk::core::error::runtime::Undefined& e) + { + EXPECT_EQ("{Frame} is undefined.", e.getMessage()); + throw; + } + }, + ostk::core::error::runtime::Undefined + ); + } + + { + const Segment::Solution segmentSolution = + Segment::Solution(defaultName_, defaultDynamics_, {}, true, Segment::Type::Maneuver); + + EXPECT_THROW( + { + try + { + segmentSolution.extractManeuvers(defaultFrameSPtr_); + } + catch (const ostk::core::error::RuntimeError& e) + { + EXPECT_EQ("No states exist within Segment Solution.", e.getMessage()); + throw; + } + }, ostk::core::error::RuntimeError ); } { const Segment::Solution segmentSolution = Segment::Solution( - defaultName_, defaultDynamics_, {initialStateWithMass_, finalStateWithMass_}, true, Segment::Type::Coast + defaultName_, defaultDynamics_, {initialStateWithMass_, finalStateWithMass_}, true, Segment::Type::Maneuver ); - EXPECT_EQ(0, segmentSolution.extractManeuvers(defaultFrameSPtr_).getSize()); + EXPECT_THROW( + { + try + { + segmentSolution.extractManeuvers(defaultFrameSPtr_); + } + catch (const ostk::core::error::RuntimeError& e) + { + EXPECT_EQ("No Thruster dynamics found in Maneuvering segment.", e.getMessage()); + throw; + } + }, + ostk::core::error::RuntimeError + ); } - // Check that it throws when no thruster dynamics are found in a maneuvering segment { const Segment::Solution segmentSolution = Segment::Solution( - defaultName_, defaultDynamics_, {initialStateWithMass_, finalStateWithMass_}, true, Segment::Type::Maneuver + defaultName_, + defaultDynamics_ + Array> {defaultThrusterDynamicsSPtr_, defaultThrusterDynamicsSPtr_}, + {initialStateWithMass_, finalStateWithMass_}, + true, + Segment::Type::Maneuver ); EXPECT_THROW( @@ -304,9 +429,9 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Extrac { segmentSolution.extractManeuvers(defaultFrameSPtr_); } - catch (const ostk::core::error::runtime::Undefined& e) + catch (const ostk::core::error::RuntimeError& e) { - EXPECT_EQ("No Thruster dynamics found in Maneuvering segment.", e.getMessage()); + EXPECT_EQ("Multiple Thruster dynamics found in Maneuvering segment.", e.getMessage()); throw; } }, @@ -314,6 +439,14 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Extrac ); } + { + const Segment::Solution segmentSolution = Segment::Solution( + defaultName_, defaultDynamics_, {initialStateWithMass_, finalStateWithMass_}, true, Segment::Type::Coast + ); + + EXPECT_EQ(0, segmentSolution.extractManeuvers(defaultFrameSPtr_).getSize()); + } + // Check that a maneuver can be extracted when a thruster dynamics is found in a maneuvering segment { const Shared durationCondition = std::make_shared( @@ -321,8 +454,8 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Extrac ); // Create maneuvering segment - Segment maneuverSegment = Segment::Maneuver( - "Maneuvring Segment", + Segment maneuveringSegment = Segment::Maneuver( + "Maneuvering Segment", durationCondition, defaultThrusterDynamicsSPtr_, defaultDynamics_, @@ -344,7 +477,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Extrac Instant::J2000(), initialCoordinatesWithWetMass, Frame::GCRF(), thrustCoordinateBrokerSPtr_ }; - const Segment::Solution maneuveringSegmentSolution = maneuverSegment.solve(initialStateWithWetMass); + const Segment::Solution maneuveringSegmentSolution = maneuveringSegment.solve(initialStateWithWetMass); const Array maneuvers = maneuveringSegmentSolution.extractManeuvers(defaultFrameSPtr_); EXPECT_EQ(1, maneuvers.getSize()); @@ -390,77 +523,23 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Extrac // Check that multiple (or no) maneuvers can be extracted when more than one maneuvers are carried out per // maneuvering segment { - const COE currentCOE = { - Length::Meters(6800.0e3), - 0.01, - Angle::Degrees(80.0), - Angle::Degrees(0.0), - Angle::Degrees(0.0), - Angle::Degrees(90.0), - }; - - const COE targetCOE = { - currentCOE.getSemiMajorAxis(), - currentCOE.getEccentricity(), - currentCOE.getInclination() + Angle::Degrees(0.1), - currentCOE.getRaan(), - currentCOE.getAop(), - currentCOE.getTrueAnomaly(), - }; - - const QLaw::Parameters qLawParameters = { - { - { - COE::Element::Inclination, - { - 1.0, // Control element weight - 1e-4, // Convergence threshold of 0.0001 - }, - }, - }, - 3, // M value - 4, // N value - 2, // R value - 0.01, // B value - 100, // K value - 1.0, // Periapsis weight - Length::Kilometers(6578.0), // Minimum periapsis - 0.5, // Absolute effectivity threshold - }; - - const QLaw qlaw = { - targetCOE, - EarthGravitationalModel::EGM2008.gravitationalParameter_, - qLawParameters, - QLaw::GradientStrategy::Analytical, - }; - - const Shared qLawThrusterDynamicsSPtr = - std::make_shared(defaultSatelliteSystem_, std::make_shared(qlaw)); - const Shared durationCondition = std::make_shared( RealCondition::DurationCondition(RealCondition::Criterion::AnyCrossing, Duration::Minutes(90.0)) ); // Create maneuvering segment - Segment maneuverSegment = Segment::Maneuver( - "Maneuvring Segment", + Segment maneuveringSegment = Segment::Maneuver( + "Maneuvering Segment", durationCondition, - qLawThrusterDynamicsSPtr, + defaultQLawThrusterDynamicsSPtr_, defaultDynamics_, - { - NumericalSolver::LogType::NoLog, - NumericalSolver::StepperType::RungeKuttaDopri5, - 5.0, - 1.0e-8, // Reducing tolerances so that the solving doesn't take forever - 1.0e-8, // Reducing tolerances so that the solving doesn't take forever - } + defaultNumericalSolver_ ); // Check that multiple maneuvers can be extracted when more than one maneuvers are carried out per maneuvering // segment { - const COE::CartesianState cartesianStatePair = currentCOE.getCartesianState( + const COE::CartesianState cartesianStatePair = defaultCurrentCOE_.getCartesianState( EarthGravitationalModel::EGM2008.gravitationalParameter_, defaultFrameSPtr_ ); VectorXd currentCoordinates(7); @@ -473,7 +552,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Extrac thrustCoordinateBrokerSPtr_, }; - const Segment::Solution maneuveringSegmentSolution = maneuverSegment.solve(currentState); + const Segment::Solution maneuveringSegmentSolution = maneuveringSegment.solve(currentState); const Array maneuvers = maneuveringSegmentSolution.extractManeuvers(defaultFrameSPtr_); @@ -512,7 +591,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Extrac // Check that when no thrusting is performed in a maneuvering segment that no maneuvers are outputted { // Pretend that we are already at the target COE, so no thrusting will occur - const COE::CartesianState cartesianStatePair = targetCOE.getCartesianState( + const COE::CartesianState cartesianStatePair = defaultTargetCOE_.getCartesianState( EarthGravitationalModel::EGM2008.gravitationalParameter_, defaultFrameSPtr_ ); VectorXd currentCoordinates(7); @@ -525,7 +604,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, SegmentSolution_Extrac thrustCoordinateBrokerSPtr_, }; - const Segment::Solution maneuveringSegmentSolution = maneuverSegment.solve(currentState); + const Segment::Solution maneuveringSegmentSolution = maneuveringSegment.solve(currentState); const Array maneuvers = maneuveringSegmentSolution.extractManeuvers(defaultFrameSPtr_); @@ -882,6 +961,20 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, Maneuver) } } +TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, ConstantLocalOrbitalFrameDirectionManeuver) +{ + { + EXPECT_NO_THROW(Segment::ConstantLocalOrbitalFrameDirectionManeuver( + defaultName_, + defaultInstantCondition_, + defaultThrusterDynamicsSPtr_, + defaultDynamics_, + defaultNumericalSolver_, + defaultLocalOrbitalFrameFactorySPtr_ + )); + } +} + TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, GetName) { EXPECT_EQ(defaultName_, defaultCoastSegment_.getName()); @@ -960,6 +1053,219 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, Solve) EXPECT_TRUE(solution.states.getSize() > 0); EXPECT_FALSE(solution.conditionIsSatisfied); } + + // Check maximum allowed angular offset behaviour + { + Segment maneuveringSegment = Segment::Maneuver( + "Maneuvering Segment", + std::make_shared( + RealCondition::DurationCondition(RealCondition::Criterion::AnyCrossing, Duration::Minutes(45.0)) + ), + defaultQLawThrusterDynamicsSPtr_, + defaultDynamics_, + defaultNumericalSolver_ + ); + + const COE::CartesianState cartesianStatePair = defaultCurrentCOE_.getCartesianState( + EarthGravitationalModel::EGM2008.gravitationalParameter_, defaultFrameSPtr_ + ); + VectorXd currentCoordinates(7); + currentCoordinates << cartesianStatePair.first.accessCoordinates(), + cartesianStatePair.second.accessCoordinates(), 200.0; + const State currentState = { + Instant::J2000(), + currentCoordinates, + defaultFrameSPtr_, + thrustCoordinateBrokerSPtr_, + }; + + const Maneuver maneuver = maneuveringSegment.solve(currentState).extractManeuvers(defaultFrameSPtr_)[0]; + const Maneuver::MeanDirectionAndMaximumAngularOffset meanDirectionAndMaximumAngularOffset = + maneuver.calculateMeanThrustDirectionAndMaximumAngularOffset(defaultLocalOrbitalFrameFactorySPtr_); + + // Maximum allowed angular offset is not provided + { + Segment constantLofDirectionManeuveringSegment = Segment::ConstantLocalOrbitalFrameDirectionManeuver( + "Constant Local Orbital Frame Direction Maneuver Segment", + std::make_shared( + RealCondition::DurationCondition(RealCondition::Criterion::AnyCrossing, Duration::Minutes(45.0)) + ), + defaultQLawThrusterDynamicsSPtr_, + defaultDynamics_, + defaultNumericalSolver_, + defaultLocalOrbitalFrameFactorySPtr_ + ); + + const Segment::Solution constantLofDirectionManeuveringSegmentSolution = + constantLofDirectionManeuveringSegment.solve(currentState); + + EXPECT_TRUE(constantLofDirectionManeuveringSegmentSolution.conditionIsSatisfied); + } + + // Maximum allowed angular offset is provided but not violated + { + Segment constantLofDirectionManeuveringSegment = Segment::ConstantLocalOrbitalFrameDirectionManeuver( + "Constant Local Orbital Frame Direction Maneuver Segment", + std::make_shared( + RealCondition::DurationCondition(RealCondition::Criterion::AnyCrossing, Duration::Minutes(45.0)) + ), + defaultQLawThrusterDynamicsSPtr_, + defaultDynamics_, + defaultNumericalSolver_, + defaultLocalOrbitalFrameFactorySPtr_, + Angle::Degrees(1.1 * meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0)) + ); + + const Segment::Solution constantLofDirectionManeuveringSegmentSolution = + constantLofDirectionManeuveringSegment.solve(currentState); + + EXPECT_TRUE(constantLofDirectionManeuveringSegmentSolution.conditionIsSatisfied); + } + + // Maximum allowed angular offset is provided and violated + { + Segment constantLofDirectionManeuveringSegment = Segment::ConstantLocalOrbitalFrameDirectionManeuver( + "Constant Local Orbital Frame Direction Maneuver Segment", + std::make_shared( + RealCondition::DurationCondition(RealCondition::Criterion::AnyCrossing, Duration::Minutes(45.0)) + ), + defaultQLawThrusterDynamicsSPtr_, + defaultDynamics_, + defaultNumericalSolver_, + defaultLocalOrbitalFrameFactorySPtr_, + Angle::Degrees(0.9 * meanDirectionAndMaximumAngularOffset.second.inDegrees(0.0, 360.0)) + ); + + EXPECT_THROW( + try { + constantLofDirectionManeuveringSegment.solve(currentState); + } catch (const ostk::core::error::RuntimeError& e) { + EXPECT_NE(e.getMessage().find("Maximum angular offset"), std::string::npos); + EXPECT_NE(e.getMessage().find(" is greater than the maximum allowed ("), std::string::npos); + throw; + }, + ostk::core::error::RuntimeError + ); + } + } + + // Multiple maneuvers + { + Segment maneuveringSegment = Segment::Maneuver( + "Maneuvering Segment", + std::make_shared( + RealCondition::DurationCondition(RealCondition::Criterion::AnyCrossing, Duration::Minutes(90.0)) + ), + defaultQLawThrusterDynamicsSPtr_, + defaultDynamics_, + defaultNumericalSolver_ + ); + + Segment constantLofDirectionManeuveringSegment = Segment::ConstantLocalOrbitalFrameDirectionManeuver( + "Constant Local Orbital Frame Direction Maneuver Segment", + std::make_shared( + RealCondition::DurationCondition(RealCondition::Criterion::AnyCrossing, Duration::Minutes(90.0)) + ), + defaultQLawThrusterDynamicsSPtr_, + defaultDynamics_, + defaultNumericalSolver_, + defaultLocalOrbitalFrameFactorySPtr_ + ); + + const COE::CartesianState cartesianStatePair = defaultCurrentCOE_.getCartesianState( + EarthGravitationalModel::EGM2008.gravitationalParameter_, defaultFrameSPtr_ + ); + VectorXd currentCoordinates(7); + currentCoordinates << cartesianStatePair.first.accessCoordinates(), + cartesianStatePair.second.accessCoordinates(), 200.0; + const State currentState = { + Instant::J2000(), + currentCoordinates, + defaultFrameSPtr_, + thrustCoordinateBrokerSPtr_, + }; + + const Segment::Solution maneuveringSegmentSolution = maneuveringSegment.solve(currentState); + const Segment::Solution constantLofDirectionManeuveringSegmentSolution = + constantLofDirectionManeuveringSegment.solve(currentState); + + const Array maneuvers = maneuveringSegmentSolution.extractManeuvers(defaultFrameSPtr_); + const Array constantLofDirectionManeuvers = + constantLofDirectionManeuveringSegmentSolution.extractManeuvers(defaultFrameSPtr_); + + EXPECT_TRUE(maneuveringSegmentSolution.accessStartInstant().isNear( + constantLofDirectionManeuveringSegmentSolution.accessStartInstant(), Duration::Milliseconds(0.0) + )); + EXPECT_TRUE(maneuveringSegmentSolution.accessEndInstant().isNear( + constantLofDirectionManeuveringSegmentSolution.accessEndInstant(), Duration::Milliseconds(1.0) + )); + EXPECT_TRUE(constantLofDirectionManeuveringSegmentSolution.conditionIsSatisfied); + EXPECT_TRUE(maneuveringSegmentSolution.conditionIsSatisfied); + EXPECT_EQ(2, maneuvers.getSize()); + EXPECT_EQ(2, constantLofDirectionManeuvers.getSize()); + + for (Size i = 0; i < maneuvers.getSize(); i++) + { + EXPECT_TRUE(maneuvers[i].getInterval().getStart().isNear( + constantLofDirectionManeuvers[i].getInterval().getStart(), Duration::Seconds(1.5) + )); + EXPECT_TRUE(maneuvers[i].getInterval().getEnd().isNear( + constantLofDirectionManeuvers[i].getInterval().getEnd(), Duration::Seconds(1.5) + )); + } + + const Shared heterogeneousGuidanceLaw = + std::make_shared(HeterogeneousGuidanceLaw()); + for (Size i = 0; i < maneuvers.getSize(); i++) + { + heterogeneousGuidanceLaw->addGuidanceLaw( + std::make_shared( + ConstantThrust::FromManeuver(maneuvers[i], defaultLocalOrbitalFrameFactorySPtr_) + ), + maneuvers[i].getInterval() + ); + } + + Segment expectedEquivalentSegment = Segment::Maneuver( + "Expected Equivalent Maneuvering Segment", + std::make_shared( + RealCondition::DurationCondition(RealCondition::Criterion::AnyCrossing, Duration::Minutes(90.0)) + ), + std::make_shared( + defaultQLawThrusterDynamicsSPtr_->getSatelliteSystem(), + heterogeneousGuidanceLaw, + defaultQLawThrusterDynamicsSPtr_->getName() + " (With Heterogeneous Guidance Law)" + ), + defaultDynamics_, + defaultNumericalSolver_ + ); + + const Segment::Solution expectedEquivalentSegmentSolution = expectedEquivalentSegment.solve(currentState); + const Array expectedEquivalentManeuvers = + expectedEquivalentSegmentSolution.extractManeuvers(defaultFrameSPtr_); + EXPECT_TRUE(maneuveringSegmentSolution.accessStartInstant().isNear( + expectedEquivalentSegmentSolution.accessStartInstant(), Duration::Milliseconds(0.0) + )); + EXPECT_TRUE(maneuveringSegmentSolution.accessEndInstant().isNear( + expectedEquivalentSegmentSolution.accessEndInstant(), Duration::Milliseconds(1.0) + )); + EXPECT_TRUE(expectedEquivalentSegmentSolution.conditionIsSatisfied); + + EXPECT_EQ(2, expectedEquivalentManeuvers.getSize()); + for (Size i = 0; i < expectedEquivalentManeuvers.getSize(); i++) + { + EXPECT_TRUE(expectedEquivalentManeuvers[i].getInterval().getStart().isNear( + maneuvers[i].getInterval().getStart(), Duration::Seconds(1.5) + )); + EXPECT_TRUE(expectedEquivalentManeuvers[i].getInterval().getEnd().isNear( + maneuvers[i].getInterval().getEnd(), Duration::Seconds(1.5) + )); + } + + const State finalState = constantLofDirectionManeuveringSegmentSolution.states.accessLast(); + const State expectedFinalState = expectedEquivalentSegmentSolution.states.accessLast(); + EXPECT_EQ(finalState, expectedFinalState); + } } TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Segment, Print)