diff --git a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.hpp b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.hpp index 66cf2db4a..6b9d94194 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.hpp @@ -32,6 +32,7 @@ #include #include #include +#include namespace ostk { @@ -55,6 +56,7 @@ using ostk::physics::time::Instant; using ostk::astro::trajectory::state::NumericalSolver; using ostk::astro::EventCondition; using ostk::astro::trajectory::State; +using ostk::astro::trajectory::StateBuilder; using ostk::astro::Dynamics; using ostk::astro::flight::system::SatelliteSystem; diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Propagated.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Propagated.cpp index 64ddd2e49..ac9828fe6 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Propagated.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Propagated.cpp @@ -27,7 +27,6 @@ using ostk::physics::units::Time; static const Derived::Unit GravitationalParameterSIUnit = Derived::Unit::GravitationalParameter(Length::Unit::Meter, Time::Unit::Second); -static const Shared gcrfSPtr = Frame::GCRF(); Propagated::Propagated(const Propagator& aPropagator, const State& aState) : Model(), @@ -140,6 +139,9 @@ Array Propagated::calculateStatesAt(const Array& anInstantArray) } } + // Builder for output states based on cached array + StateBuilder outputStateBuilder = {this->cachedStateArray_.accessFirst()}; + Array allStates = Array::Empty(); // Maintain counter separately so as to only iterate once through instant array @@ -213,12 +215,12 @@ Array Propagated::calculateStatesAt(const Array& anInstantArray) (forwardStates[k].accessCoordinates() * forwardWeight + backwardStates[k].accessCoordinates() * backwardWeight); - averagedStates.add({ - instants[k], - coordinates, - gcrfSPtr, - propagator_.accessCoordinatesBroker(), - }); + averagedStates.add( + outputStateBuilder.build( + instants[k], + coordinates + ) + ); } allStates.add(averagedStates); diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.cpp index a2af230c2..08f2b2fbc 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.cpp @@ -143,18 +143,21 @@ State Propagator::calculateStateAt(const State& aState, const Instant& anInstant throw ostk::core::error::runtime::Undefined("Propagator"); } - const State state = { - aState.accessInstant(), - aState.inFrame(Propagator::IntegrationFrameSPtr).extractCoordinates(this->coordinatesBrokerSPtr_->getSubsets()), - Propagator::IntegrationFrameSPtr, - this->coordinatesBrokerSPtr_, - }; + const StateBuilder solverStateBuilder = {Propagator::IntegrationFrameSPtr, coordinatesBrokerSPtr_}; + + const State solverInputState = solverStateBuilder.reduce(aState.inFrame(Propagator::IntegrationFrameSPtr)); - return numericalSolver_.integrateTime( - state, + const State solverOutputState = numericalSolver_.integrateTime( + solverInputState, anInstant, - Dynamics::GetSystemOfEquations(this->dynamicsContexts_, state.accessInstant(), Propagator::IntegrationFrameSPtr) + Dynamics::GetSystemOfEquations( + this->dynamicsContexts_, solverInputState.accessInstant(), Propagator::IntegrationFrameSPtr + ) ); + + const StateBuilder outputStateBuilder = {aState}; + + return outputStateBuilder.expand(solverOutputState.inFrame(aState.accessFrame()), aState); } NumericalSolver::ConditionSolution Propagator::calculateStateToCondition( @@ -168,20 +171,21 @@ NumericalSolver::ConditionSolution Propagator::calculateStateToCondition( const Instant startInstant = aState.getInstant(); - const State state = { - aState.accessInstant(), - aState.inFrame(Propagator::IntegrationFrameSPtr).extractCoordinates(this->coordinatesBrokerSPtr_->getSubsets()), - Propagator::IntegrationFrameSPtr, - this->coordinatesBrokerSPtr_, - }; + const StateBuilder solverStateBuilder = {Propagator::IntegrationFrameSPtr, coordinatesBrokerSPtr_}; + + const State solverInputState = solverStateBuilder.reduce(aState.inFrame(Propagator::IntegrationFrameSPtr)); - const NumericalSolver::ConditionSolution conditionSolution = numericalSolver_.integrateTime( - state, + NumericalSolver::ConditionSolution conditionSolution = numericalSolver_.integrateTime( + solverInputState, anInstant, Dynamics::GetSystemOfEquations(this->dynamicsContexts_, startInstant, Propagator::IntegrationFrameSPtr), anEventCondition ); + const StateBuilder outputStateBuilder = {aState}; + + conditionSolution.state = outputStateBuilder.expand(conditionSolution.state.inFrame(aState.accessFrame()), aState); + return conditionSolution; } @@ -210,20 +214,19 @@ Array Propagator::calculateStatesAt(const State& aState, const ArraycoordinatesBrokerSPtr_->getSubsets()), - Propagator::IntegrationFrameSPtr, - this->coordinatesBrokerSPtr_, - }; + const StateBuilder solverStateBuilder = {Propagator::IntegrationFrameSPtr, coordinatesBrokerSPtr_}; + + const State solverInputState = solverStateBuilder.reduce(aState.inFrame(Propagator::IntegrationFrameSPtr)); - const Instant startInstant = state.accessInstant(); + const Instant startInstant = solverInputState.accessInstant(); Array forwardInstants; forwardInstants.reserve(anInstantArray.getSize()); Array backwardInstants; backwardInstants.reserve(anInstantArray.getSize()); + const StateBuilder outputStateBuilder(aState); + for (const Instant& anInstant : anInstantArray) { if (anInstant <= startInstant) @@ -241,7 +244,7 @@ Array Propagator::calculateStatesAt(const State& aState, const ArraydynamicsContexts_, startInstant, Propagator::IntegrationFrameSPtr) ); @@ -254,7 +257,7 @@ Array Propagator::calculateStatesAt(const State& aState, const ArraydynamicsContexts_, startInstant, Propagator::IntegrationFrameSPtr) ); @@ -262,7 +265,15 @@ Array Propagator::calculateStatesAt(const State& aState, const Array outputStates; + outputStates.reserve(backwardPropagatedStates.getSize() + forwardPropagatedStates.getSize()); + + for (const State& solverOutputState : backwardPropagatedStates + forwardPropagatedStates) + { + outputStates.add(outputStateBuilder.expand(solverOutputState.inFrame(aState.accessFrame()), aState)); + } + + return outputStates; } void Propagator::print(std::ostream& anOutputStream, bool displayDecorator) const diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.test.cpp index 46b7f9cd5..5f6153922 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.test.cpp @@ -400,6 +400,66 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator, Calcul { EXPECT_NO_THROW(defaultPropagator_.calculateStateAt(state, instant)); } + + { + // Test that the propagator respects the input Frame + const State outputStateGCRF = defaultPropagator_.calculateStateAt(state, instantArray[0]); + const State outputStateITRF = + defaultPropagator_.calculateStateAt(state.inFrame(Frame::ITRF()), instantArray[0]); + + EXPECT_EQ(outputStateGCRF.getFrame(), Frame::GCRF()); + EXPECT_EQ(outputStateITRF.getFrame(), Frame::ITRF()); + + const State outputStateGCRF2 = outputStateITRF.inFrame(Frame::GCRF()); + + EXPECT_EQ(outputStateGCRF.getInstant(), outputStateGCRF2.getInstant()); + EXPECT_EQ(outputStateGCRF.getFrame(), outputStateGCRF2.getFrame()); + EXPECT_EQ(outputStateGCRF.accessCoordinatesBroker(), outputStateGCRF2.accessCoordinatesBroker()); + + // Get the coordinates from the two states + const VectorXd coordinates1 = outputStateGCRF.getCoordinates(); + const VectorXd coordinates2 = outputStateGCRF2.getCoordinates(); + + // Check that the two vectors have the same size + EXPECT_EQ(coordinates1.size(), coordinates2.size()); + + // Check that each element of the two vectors is close + for (int i = 0; i < coordinates1.size(); ++i) + { + EXPECT_NEAR(coordinates1(i), coordinates2(i), 1e-5); + } + } + + { + // Test that the propagator respects the input State dimensions + + VectorXd coords(8); + coords << 7000000.0, 0.0, 0.0, 0.0, 5335.865450622126, 5335.865450622126, 1, 2; + + const Array> coordinatesSubsets = { + CartesianPosition::Default(), + CartesianVelocity::Default(), + std::make_shared(CoordinatesSubset("extra1", 1)), + std::make_shared(CoordinatesSubset("extra2", 1)), + }; + + const State bigState = { + Instant::DateTime(DateTime(2018, 1, 2, 0, 0, 0), Scale::UTC), + coords, + gcrfSPtr_, + coordinatesSubsets, + }; + + // Confirm the propagator only needs 6 dimensions + EXPECT_EQ(defaultPropagator_.getNumberOfCoordinates(), 6); + EXPECT_EQ(bigState.getCoordinates().size(), 8); + + const State outputState = defaultPropagator_.calculateStateAt(bigState, instantArray[0]); + + EXPECT_NE(bigState, outputState); + EXPECT_EQ(bigState.getCoordinatesSubsets(), outputState.getCoordinatesSubsets()); + EXPECT_EQ(outputState.getCoordinates().size(), 8); + } } TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator, CalculateStateAt_Condition) @@ -438,6 +498,100 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator, Calcul EXPECT_FALSE(propagator.calculateStateToCondition(state, endInstant, failureCondition).conditionIsSatisfied); } + + { + // Current state and instant setup + const State state = { + Instant::DateTime(DateTime(2018, 1, 2, 0, 0, 0), Scale::UTC), + Position::Meters({7000000.0, 0.0, 0.0}, gcrfSPtr_), + Velocity::MetersPerSecond({0.0, 5335.865450622126, 5335.865450622126}, gcrfSPtr_), + }; + + // Setup instants + const Instant endInstant = Instant::DateTime(DateTime(2018, 1, 2, 1, 0, 0), Scale::UTC); + + const InstantCondition condition = { + InstantCondition::Criterion::StrictlyPositive, + state.accessInstant() + Duration::Seconds(60.0), + }; + + const Propagator propagator = {defaultRKD5_, defaultDynamics_}; + + const NumericalSolver::ConditionSolution conditionSolutionGCRF = + propagator.calculateStateToCondition(state, endInstant, condition); + + const NumericalSolver::ConditionSolution conditionSolutionITRF = + propagator.calculateStateToCondition(state.inFrame(Frame::ITRF()), endInstant, condition); + + // Test that the propagator respects the input Frame + const State outputStateGCRF = conditionSolutionGCRF.state; + const State outputStateITRF = conditionSolutionITRF.state; + + EXPECT_EQ(outputStateGCRF.getFrame(), Frame::GCRF()); + EXPECT_EQ(outputStateITRF.getFrame(), Frame::ITRF()); + + const State outputStateGCRF2 = outputStateITRF.inFrame(Frame::GCRF()); + + EXPECT_EQ(outputStateGCRF.getFrame(), outputStateGCRF2.getFrame()); + EXPECT_EQ(outputStateGCRF.accessCoordinatesBroker(), outputStateGCRF2.accessCoordinatesBroker()); + + // Get the coordinates from the two states + const VectorXd coordinates1 = outputStateGCRF.getCoordinates(); + const VectorXd coordinates2 = outputStateGCRF2.getCoordinates(); + + // Check that the two vectors have the same size + EXPECT_EQ(coordinates1.size(), coordinates2.size()); + + // Check that each element of the two vectors is close + for (int i = 0; i < coordinates1.size(); ++i) + { + EXPECT_NEAR(coordinates1(i), coordinates2(i), 1e-5); + } + } + + { + // Test that the propagator respects the input State dimensions + + VectorXd coords(8); + coords << 7000000.0, 0.0, 0.0, 0.0, 5335.865450622126, 5335.865450622126, 1, 2; + + const Array> coordinatesSubsets = { + CartesianPosition::Default(), + CartesianVelocity::Default(), + std::make_shared(CoordinatesSubset("extra1", 1)), + std::make_shared(CoordinatesSubset("extra2", 1)), + }; + + const State bigState = { + Instant::DateTime(DateTime(2018, 1, 2, 0, 0, 0), Scale::UTC), + coords, + gcrfSPtr_, + coordinatesSubsets, + }; + + // Setup instants + const Instant endInstant = Instant::DateTime(DateTime(2018, 1, 2, 1, 0, 0), Scale::UTC); + + const InstantCondition condition = { + InstantCondition::Criterion::StrictlyPositive, + bigState.accessInstant() + Duration::Seconds(60.0), + }; + + const Propagator propagator = {defaultRKD5_, defaultDynamics_}; + + // Confirm the propagator only needs 6 dimensions + EXPECT_EQ(propagator.getNumberOfCoordinates(), 6); + EXPECT_EQ(bigState.getCoordinates().size(), 8); + + const NumericalSolver::ConditionSolution conditionSolution = + propagator.calculateStateToCondition(bigState, endInstant, condition); + + const State outputState = conditionSolution.state; + + EXPECT_NE(bigState, outputState); + EXPECT_EQ(bigState.getCoordinatesSubsets(), outputState.getCoordinatesSubsets()); + EXPECT_EQ(outputState.getCoordinates().size(), 8); + } } TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator, CalculateStatesAt) @@ -597,6 +751,80 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator, Calcul // std::cout << "**************************************" << std::endl; } } + + { + // Current state and instant setup + const State state = { + Instant::DateTime(DateTime(2018, 1, 2, 0, 0, 0), Scale::UTC), + Position::Meters({7000000.0, 0.0, 0.0}, gcrfSPtr_), + Velocity::MetersPerSecond({0.0, 5335.865450622126, 5335.865450622126}, gcrfSPtr_), + }; + + // Test that the propagator respects the input Frame + const Array outputStatesGCRF = defaultPropagator_.calculateStatesAt(state, instantArray); + const Array outputStatesITRF = + defaultPropagator_.calculateStatesAt(state.inFrame(Frame::ITRF()), instantArray); + + // Validation loop + for (size_t i = 0; i < outputStatesGCRF.getSize(); i++) + { + EXPECT_EQ(outputStatesGCRF[i].getFrame(), Frame::GCRF()); + EXPECT_EQ(outputStatesITRF[i].getFrame(), Frame::ITRF()); + + const State outputStateGCRF2 = outputStatesITRF[i].inFrame(Frame::GCRF()); + + EXPECT_EQ(outputStatesGCRF[i].getInstant(), outputStateGCRF2.getInstant()); + EXPECT_EQ(outputStatesGCRF[i].getFrame(), outputStateGCRF2.getFrame()); + EXPECT_EQ(outputStatesGCRF[i].accessCoordinatesBroker(), outputStateGCRF2.accessCoordinatesBroker()); + + // Get the coordinates from the two states + const VectorXd coordinates1 = outputStatesGCRF[i].getCoordinates(); + const VectorXd coordinates2 = outputStateGCRF2.getCoordinates(); + + // Check that the two vectors have the same size + EXPECT_EQ(coordinates1.size(), coordinates2.size()); + + // Check that each element of the two vectors is close + for (int j = 0; j < coordinates1.size(); ++j) + { + EXPECT_NEAR(coordinates1(j), coordinates2(j), 1e-3); + } + } + } + + { + // Test that the propagator respects the input State dimensions + + VectorXd coords(8); + coords << 7000000.0, 0.0, 0.0, 0.0, 5335.865450622126, 5335.865450622126, 1, 2; + + const Array> coordinatesSubsets = { + CartesianPosition::Default(), + CartesianVelocity::Default(), + std::make_shared(CoordinatesSubset("extra1", 1)), + std::make_shared(CoordinatesSubset("extra2", 1)), + }; + + const State bigState = { + Instant::DateTime(DateTime(2018, 1, 2, 0, 0, 0), Scale::UTC), + coords, + gcrfSPtr_, + coordinatesSubsets, + }; + + // Confirm the propagator only needs 6 dimensions + EXPECT_EQ(defaultPropagator_.getNumberOfCoordinates(), 6); + EXPECT_EQ(bigState.getCoordinates().size(), 8); + + const Array outputStates = defaultPropagator_.calculateStatesAt(bigState, instantArray); + + for (size_t i = 0; i < outputStates.getSize(); i++) + { + EXPECT_NE(bigState, outputStates[i]); + EXPECT_EQ(bigState.getCoordinatesSubsets(), outputStates[i].getCoordinatesSubsets()); + EXPECT_EQ(outputStates[i].getCoordinates().size(), 8); + } + } } TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator, OrekitSpherical) @@ -2215,7 +2443,8 @@ TEST_F( SWManager::Get().reset(); } -// TBI: Specific class for parameterized tests on thruster dynamics, could inherit from base fixture for common inputs +// TBI: Specific class for parameterized tests on thruster dynamics, could inherit from base fixture for common + // error: 'testing::Test' is an ambiguous base class OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Models_Propagator_Thruster : public ::testing::TestWithParam< @@ -2511,16 +2740,21 @@ TEST_P( // std::cout << "Mass error is: " << massError << "kg" << std::endl; // std::cout << "Maneuver acceleration error GCRF X is: " << maneuverContributionGCRF[0] - // referenceManeuverAccelerationArrayGCRF[i][0] << "m/s^2" << std::endl; std::cout << "Maneuver acceleration - // error GCRF Y is: " << maneuverContributionGCRF[1] - referenceManeuverAccelerationArrayGCRF[i][1] << "m/s^2" + // error GCRF Y is: " << maneuverContributionGCRF[1] - referenceManeuverAccelerationArrayGCRF[i][1] << + // "m/s^2" // << std::endl; std::cout << "Maneuver acceleration error GCRF Z is: " << maneuverContributionGCRF[2] - // referenceManeuverAccelerationArrayGCRF[i][2] << "m/s^2" << std::endl; std::cout << "Maneuver acceleration - // error LOF X is: " << maneuverContributionLOF[0] - referenceManeuverAccelerationArrayLOF[i][0] << "m/s^2" << + // error LOF X is: " << maneuverContributionLOF[0] - referenceManeuverAccelerationArrayLOF[i][0] << "m/s^2" + // << // std::endl; std::cout << "Maneuver acceleration error LOF Y is: " << maneuverContributionLOF[1] - // referenceManeuverAccelerationArrayLOF[i][1] << "m/s^2" << std::endl; std::cout << "Maneuver acceleration - // error LOF Z is: " << maneuverContributionLOF[2] - referenceManeuverAccelerationArrayLOF[i][2] << "m/s^2" << - // std::endl; std::cout << "Maneuver acceleration error GCRF is: " << maneuverAccelerationContributionErrorGCRF + // error LOF Z is: " << maneuverContributionLOF[2] - referenceManeuverAccelerationArrayLOF[i][2] << "m/s^2" + // << + // std::endl; std::cout << "Maneuver acceleration error GCRF is: " << + // maneuverAccelerationContributionErrorGCRF // << "m/s^2" << std::endl; // Do it in percentage std::cout << "Maneuver acceleration error LOF is: " << - // maneuverAccelerationContributionErrorLOF << "m/s^2" << std::endl; std::cout << "Total acceleration (central + // maneuverAccelerationContributionErrorLOF << "m/s^2" << std::endl; std::cout << "Total acceleration + // (central // body + maneuver) error GCRF is: " << totalAccelerationErrorGCRF << "m/s^2" << std::endl; std::cout << // "Central body acceleration contribution error GCRF is: " << // centralBodyGravityAccelerationContributionErrorGCRF << "m/s^2" << std::endl; @@ -2599,7 +2833,7 @@ INSTANTIATE_TEST_SUITE_P( "/app/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Models/Propagated/" "Orekit_ConstantThrustThruster_7000000.0_98.1_2023-01-01T00-00-00.000_115.0_0.1_1500.0_3600.0_LVLH_1.0_0.0_" "0.0_30.0.csv", // Scenario validation data file path - LocalOrbitalFrameFactory::LVLH(Frame::GCRF()), // Local Orbital Frame Factory to express thrust direction + LocalOrbitalFrameFactory::LVLH(Frame::GCRF()), // Local Orbital Frame Factory to express thrust Vector3d({1.0, 0.0, 0.0}), // Thrust direction in Local Orbital Frame 100.0, // Satellite dry mass [kg] 0.1, // Thrust [N] @@ -3059,7 +3293,8 @@ TEST_P( // std::cout << "Mass error is: " << massError << "kg" << std::endl; // std::cout << "Maneuver acceleration error GCRF X is: " << maneuverContributionGCRF[0] - // referenceManeuverAccelerationArrayGCRF[i][0] << "m/s^2" << std::endl; std::cout << "Maneuver acceleration - // error GCRF Y is: " << maneuverContributionGCRF[1] - referenceManeuverAccelerationArrayGCRF[i][1] << "m/s^2" + // error GCRF Y is: " << maneuverContributionGCRF[1] - referenceManeuverAccelerationArrayGCRF[i][1] << + // "m/s^2" // << std::endl; std::cout << "Maneuver acceleration error GCRF Z is: " << maneuverContributionGCRF[2] - // referenceManeuverAccelerationArrayGCRF[i][2] << "m/s^2" << std::endl; std::cout << "Maneuver acceleration // error GCRF is: " << maneuverAccelerationContributionErrorGCRF << "m/s^2" << std::endl; // Do it in @@ -3068,7 +3303,8 @@ TEST_P( // std::cout << "Drag acceleration error GCRF is: " << dragAccelerationContributionErrorGCRF << "m/s^2" << // std::endl; // Do it in percentage std::cout << "Total acceleration (centralbody + maneuver + drag) error // GCRF is: " << totalAccelerationErrorGCRF << "m/s^2" << std::endl; - // std::cout.setf(std::ios::fixed,std::ios::floatfield); std::cout << "**************************************" + // std::cout.setf(std::ios::fixed,std::ios::floatfield); std::cout << + // "**************************************" // << std::endl; } }