From 87ea1eab1b4f651f37ae85fac9fcbc98835abf2e Mon Sep 17 00:00:00 2001 From: Vishwa Shah Date: Mon, 23 Sep 2024 13:54:37 -0700 Subject: [PATCH] feat: always return states in gcrf for trajectory models (#439) * feat: always return states in GCRF * feat: add tests * feat: fix python tests * Apply suggestions from code review Co-authored-by: Antoine Paletta <98616558+apaletta3@users.noreply.github.com> * feat: address feedback --------- Co-authored-by: Antoine Paletta <98616558+apaletta3@users.noreply.github.com> --- .../Trajectory.cpp | 2 +- bindings/python/test/conftest.py | 4 +- bindings/python/test/flight/test_profile.py | 2 +- .../Astrodynamics/Trajectory/Model/Static.hpp | 95 ++++++++++- .../Flight/Profile/Model/Tabulated.cpp | 55 +++--- .../Astrodynamics/Trajectory/Model/Static.cpp | 7 +- .../Trajectory/Model/Tabulated.cpp | 2 +- .../Flight/Profile/Tabulated.test.cpp | 2 + .../Astrodynamics/Trajectory.test.cpp | 160 ++++-------------- .../Trajectory/Orbit/Model/Tabulated.test.cpp | 2 + 10 files changed, 170 insertions(+), 161 deletions(-) diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory.cpp index 988689f4c..3cd5b517f 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory.cpp @@ -125,7 +125,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory(pybind11::module& aModule Create a `Trajectory` object representing a position. Args: - position (Position): The position. + position (Position): The position. Must be in the ITRF frame. Returns: Trajectory: The `Trajectory` object representing the position. diff --git a/bindings/python/test/conftest.py b/bindings/python/test/conftest.py index a1c4a4055..5fcf69ac2 100644 --- a/bindings/python/test/conftest.py +++ b/bindings/python/test/conftest.py @@ -115,5 +115,5 @@ def aer(azimuth: Angle, elevation: Angle, range: Length) -> LLA: @pytest.fixture -def trajectory(position: Position) -> Trajectory: - return Trajectory.position(position) +def trajectory(position: Position, instant_1: Instant) -> Trajectory: + return Trajectory.position(position.in_frame(Frame.ITRF(), instant_1)) diff --git a/bindings/python/test/flight/test_profile.py b/bindings/python/test/flight/test_profile.py index 1c05c34f1..da387f95d 100644 --- a/bindings/python/test/flight/test_profile.py +++ b/bindings/python/test/flight/test_profile.py @@ -172,7 +172,7 @@ def test_inertial_pointing(self): quaternion: Quaternion = Quaternion([0.0, 0.0, 0.0, 1.0], Quaternion.Format.XYZS) trajectory: Trajectory = Trajectory.position( - Position.meters((0.0, 0.0, 0.0), Frame.GCRF()) + Position.meters((0.0, 0.0, 0.0), Frame.ITRF()) ) profile: Profile = Profile.inertial_pointing(trajectory, quaternion) diff --git a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Static.hpp b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Static.hpp index 4763d6ccc..717d1533e 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Static.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Static.hpp @@ -25,29 +25,122 @@ using ostk::physics::time::Interval; using ostk::astrodynamics::trajectory::Model; using ostk::astrodynamics::trajectory::State; -/// @brief Static trajectory model +/// @brief Static trajectory model to represent a fixed position on the surface of the earth class Static : public virtual Model { public: + /// @brief Constructor + /// + /// @code{.cpp} + /// Position position = { ... } ; + /// Static staticModel(position) ; + /// @endcode + /// + /// @param aPosition The position of the static model. Must be provided in the ITRF frame. Static(const Position& aPosition); + /// @brief Clone the static model + /// + /// @code{.cpp} + /// Static staticModel = { ... } ; + /// Static* clonedModel = staticModel.clone() ; + /// @endcode + /// + /// @return A pointer to the cloned static model virtual Static* clone() const override; + /// @brief Equality operator + /// + /// @code{.cpp} + /// Static staticModel1 = { ... } ; + /// Static staticModel2 = { ... } ; + /// bool isEqual = (staticModel1 == staticModel2) ; + /// @endcode + /// + /// @param aStaticModel The static model to compare with + /// @return True if the models are equal, false otherwise bool operator==(const Static& aStaticModel) const; + /// @brief Inequality operator + /// + /// @code{.cpp} + /// Static staticModel1 = { ... } ; + /// Static staticModel2 = { ... } ; + /// bool isNotEqual = (staticModel1 != staticModel2) ; + /// @endcode + /// + /// @param aStaticModel The static model to compare with + /// @return True if the models are not equal, false otherwise bool operator!=(const Static& aStaticModel) const; + /// @brief Output stream operator + /// + /// @code{.cpp} + /// Static staticModel = { ... } ; + /// std::cout << staticModel ; + /// @endcode + /// + /// @param anOutputStream The output stream + /// @param aStaticModel The static model to output + /// @return The output stream friend std::ostream& operator<<(std::ostream& anOutputStream, const Static& aStaticModel); + /// @brief Check if the static model is defined + /// + /// @code{.cpp} + /// Static staticModel = { ... } ; + /// bool isDefined = staticModel.isDefined() ; + /// @endcode + /// + /// @return True if the model is defined, false otherwise virtual bool isDefined() const override; + /// @brief Calculate the state at a given instant + /// + /// @code{.cpp} + /// Static staticModel = { ... } ; + /// Instant instant = { ... } ; + /// State state = staticModel.calculateStateAt(instant) ; + /// @endcode + /// + /// @param anInstant The instant at which to calculate the state + /// @return The state at the given instant virtual State calculateStateAt(const Instant& anInstant) const override; + /// @brief Print the static model to an output stream + /// + /// @code{.cpp} + /// Static staticModel = { ... } ; + /// staticModel.print(std::cout, true) ; + /// @endcode + /// + /// @param anOutputStream The output stream + /// @param displayDecorator If true, display decorator virtual void print(std::ostream& anOutputStream, bool displayDecorator = true) const override; protected: + /// @brief Equality operator for Model base class + /// + /// @code{.cpp} + /// Static staticModel = { ... } ; + /// Model model = { ... } ; + /// bool isEqual = (staticModel == model) ; + /// @endcode + /// + /// @param aModel The model to compare with + /// @return True if the models are equal, false otherwise virtual bool operator==(const Model& aModel) const override; + /// @brief Inequality operator for Model base class + /// + /// @code{.cpp} + /// Static staticModel = { ... } ; + /// Model model = { ... } ; + /// bool isNotEqual = (staticModel != model) ; + /// @endcode + /// + /// @param aModel The model to compare with + /// @return True if the models are not equal, false otherwise virtual bool operator!=(const Model& aModel) const override; private: diff --git a/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/Model/Tabulated.cpp b/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/Model/Tabulated.cpp index 097b73876..8d46289e9 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/Model/Tabulated.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile/Model/Tabulated.cpp @@ -104,35 +104,36 @@ State Tabulated::calculateStateAt(const Instant& anInstant) const const Real ratio = Duration::Between(previousState.accessInstant(), anInstant).inSeconds() / Duration::Between(previousState.accessInstant(), nextState.accessInstant()).inSeconds(); - return { - anInstant, - Position { - previousState.getPosition().accessCoordinates() + - ratio * - (nextState.getPosition().accessCoordinates() - previousState.getPosition().accessCoordinates()), - previousState.getPosition().getUnit(), - previousState.getPosition().accessFrame() - }, - Velocity { - previousState.getVelocity().accessCoordinates() + - ratio * - (nextState.getVelocity().accessCoordinates() - previousState.getVelocity().accessCoordinates()), - previousState.getVelocity().getUnit(), - previousState.getVelocity().accessFrame() - }, - Quaternion::SLERP(previousState.getAttitude(), nextState.getAttitude(), ratio), - previousState.getAngularVelocity() + - ratio * (nextState.getAngularVelocity() - previousState.getAngularVelocity()), - previousState.getFrame() - }; + return State( + anInstant, + Position( + previousState.getPosition().accessCoordinates() + + ratio * (nextState.getPosition().accessCoordinates() - + previousState.getPosition().accessCoordinates()), + previousState.getPosition().getUnit(), + previousState.getPosition().accessFrame() + ), + Velocity( + previousState.getVelocity().accessCoordinates() + + ratio * (nextState.getVelocity().accessCoordinates() - + previousState.getVelocity().accessCoordinates()), + previousState.getVelocity().getUnit(), + previousState.getVelocity().accessFrame() + ), + Quaternion::SLERP(previousState.getAttitude(), nextState.getAttitude(), ratio), + Vector3d(previousState.getAngularVelocity() + + ratio * (nextState.getAngularVelocity() - previousState.getAngularVelocity())), + previousState.getFrame() + ) + .inFrame(Frame::GCRF()); } else if (stateRange.first != nullptr) { - return *(stateRange.first); + return (*(stateRange.first)).inFrame(Frame::GCRF()); } else if (stateRange.second != nullptr) { - return *(stateRange.second); + return (*(stateRange.second)).inFrame(Frame::GCRF()); } throw ostk::core::error::RuntimeError("Cannot calculate state at [{}].", anInstant.toString()); @@ -153,11 +154,11 @@ Axes Tabulated::getAxesAt(const Instant& anInstant) const } const State state = this->calculateStateAt(anInstant); - const Quaternion q_GCRF_BODY = state.getAttitude().toConjugate(); + const Quaternion q_GCRF_B = state.getAttitude().toConjugate(); - const Vector3d xAxis = q_GCRF_BODY * Vector3d::X(); - const Vector3d yAxis = q_GCRF_BODY * Vector3d::Y(); - const Vector3d zAxis = q_GCRF_BODY * Vector3d::Z(); + const Vector3d xAxis = q_GCRF_B * Vector3d::X(); + const Vector3d yAxis = q_GCRF_B * Vector3d::Y(); + const Vector3d zAxis = q_GCRF_B * Vector3d::Z(); return {xAxis, yAxis, zAxis, state.accessFrame()}; } diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Static.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Static.cpp index d9aa56056..c09ad3ab5 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Static.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Static.cpp @@ -18,6 +18,10 @@ Static::Static(const Position& aPosition) : Model(), position_(aPosition) { + if (aPosition.accessFrame() != Frame::ITRF()) + { + throw ostk::core::error::runtime::Wrong("Position Frame", aPosition.accessFrame()->getName()); + } } Static* Static::clone() const @@ -66,7 +70,8 @@ State Static::calculateStateAt(const Instant& anInstant) const throw ostk::core::error::runtime::Undefined("Static"); } - return State(anInstant, position_, Velocity::MetersPerSecond({0.0, 0.0, 0.0}, position_.accessFrame())); + return State(anInstant, position_, Velocity::MetersPerSecond({0.0, 0.0, 0.0}, position_.accessFrame())) + .inFrame(Frame::GCRF()); } void Static::print(std::ostream& anOutputStream, bool displayDecorator) const diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Tabulated.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Tabulated.cpp index 7bfcc3ebb..db4e5623c 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Tabulated.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Model/Tabulated.cpp @@ -156,7 +156,7 @@ State Tabulated::calculateStateAt(const Instant& anInstant) const const Shared& frame = firstState_.accessFrame(); const Shared& coordinatesBroker = firstState_.accessCoordinateBroker(); - return State(anInstant, interpolatedCoordinates, frame, coordinatesBroker); + return State(anInstant, interpolatedCoordinates, frame, coordinatesBroker).inFrame(Frame::GCRF()); } Array Tabulated::calculateStatesAt(const Array& anInstantArray) const diff --git a/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/Tabulated.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/Tabulated.test.cpp index 05e367293..386b740d8 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/Tabulated.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile/Tabulated.test.cpp @@ -159,6 +159,8 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile_Models_Tabulated, Calculate const Vector3d angularVelocity = {0.0, 0.0, 0.0}; EXPECT_VECTORS_ALMOST_EQUAL(state.getAngularVelocity(), angularVelocity, 1e-10); + + EXPECT_TRUE(state.getPosition().accessFrame() == Frame::GCRF()); } } diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory.test.cpp index 00a0c32cc..f1ae44a27 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory.test.cpp @@ -7,22 +7,22 @@ #include -TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, Constructor) -{ - using ostk::core::container::Array; - using ostk::core::type::Shared; +using ostk::core::container::Array; +using ostk::core::type::Shared; - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; +using ostk::physics::coordinate::Frame; +using ostk::physics::coordinate::Position; +using ostk::physics::coordinate::Velocity; +using ostk::physics::time::DateTime; +using ostk::physics::time::Instant; +using ostk::physics::time::Scale; - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; +using ostk::astrodynamics::Trajectory; +using ostk::astrodynamics::trajectory::model::Tabulated; +using ostk::astrodynamics::trajectory::State; +TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, Constructor) +{ { const Shared gcrfSPtr = Frame::GCRF(); @@ -62,20 +62,6 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, Constructor) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, EqualToOperator) { - using ostk::core::container::Array; - using ostk::core::type::Shared; - - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; - - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; - { const Shared gcrfSPtr = Frame::GCRF(); @@ -137,20 +123,6 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, EqualToOperator) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, NotEqualToOperator) { - using ostk::core::container::Array; - using ostk::core::type::Shared; - - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; - - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; - { const Shared gcrfSPtr = Frame::GCRF(); @@ -212,20 +184,6 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, NotEqualToOperator) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, StreamOperator) { - using ostk::core::container::Array; - using ostk::core::type::Shared; - - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; - - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; - { const Shared gcrfSPtr = Frame::GCRF(); @@ -252,20 +210,6 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, StreamOperator) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, IsDefined) { - using ostk::core::container::Array; - using ostk::core::type::Shared; - - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; - - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; - { const Shared gcrfSPtr = Frame::GCRF(); @@ -292,20 +236,6 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, IsDefined) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, AccessModel) { - using ostk::core::container::Array; - using ostk::core::type::Shared; - - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; - - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; - { const Shared gcrfSPtr = Frame::GCRF(); @@ -332,20 +262,6 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, AccessModel) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, GetStateAt) { - using ostk::core::container::Array; - using ostk::core::type::Shared; - - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; - - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; - { const Shared gcrfSPtr = Frame::GCRF(); @@ -402,20 +318,6 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, GetStateAt) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, GetStatesAt) { - using ostk::core::container::Array; - using ostk::core::type::Shared; - - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; - - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; - // Test correct handling of state array dimensions { const Shared gcrfSPtr = Frame::GCRF(); @@ -522,20 +424,6 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, GetStatesAt) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, Print) { - using ostk::core::container::Array; - using ostk::core::type::Shared; - - using ostk::physics::coordinate::Frame; - using ostk::physics::coordinate::Position; - using ostk::physics::coordinate::Velocity; - using ostk::physics::time::DateTime; - using ostk::physics::time::Instant; - using ostk::physics::time::Scale; - - using ostk::astrodynamics::Trajectory; - using ostk::astrodynamics::trajectory::model::Tabulated; - using ostk::astrodynamics::trajectory::State; - { const Shared gcrfSPtr = Frame::GCRF(); @@ -563,10 +451,28 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, Print) TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, Undefined) { - using ostk::astrodynamics::Trajectory; - { EXPECT_NO_THROW(Trajectory::Undefined()); EXPECT_FALSE(Trajectory::Undefined().isDefined()); } } + +TEST(OpenSpaceToolkit_Astrodynamics_Trajectory, Position) +{ + { + const Trajectory trajectory = Trajectory::Position(Position::Meters({2.0, 0.0, 0.0}, Frame::ITRF())); + EXPECT_TRUE(trajectory.isDefined()); + } + + { + EXPECT_THROW( + Trajectory::Position(Position::Meters({2.0, 0.0, 0.0}, Frame::GCRF())), ostk::core::error::runtime::Wrong + ); + } + + { + const Trajectory trajectory = Trajectory::Position(Position::Meters({2.0, 0.0, 0.0}, Frame::ITRF())); + const State state = trajectory.getStateAt(Instant::J2000()); + EXPECT_TRUE(state.accessFrame() == Frame::GCRF()); + } +} diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Model/Tabulated.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Model/Tabulated.test.cpp index ef805486b..ef3616c37 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Model/Tabulated.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/Orbit/Model/Tabulated.test.cpp @@ -216,6 +216,8 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_Orbit_Model_Tabulated, Calculat VectorXd residuals = (states[i].getCoordinates() - referenceStates[i].getCoordinates()).array().abs(); EXPECT_TRUE((residuals.array() < tolerance).all()) << String::Format("Residual: {}", residuals.maxCoeff()); + + EXPECT_TRUE(states[i].getFrame() == Frame::GCRF()); } } }