diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy.cxx b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy.cxx index 9344aae6c..1aa49318e 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy.cxx +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy.cxx @@ -38,9 +38,9 @@ PYBIND11_MODULE(OpenSpaceToolkitAstrodynamicsPy, m) OpenSpaceToolkitAstrodynamicsPy_RootSolver(m); // Add python submodules to OpenSpaceToolkitAstrodynamicsPy + OpenSpaceToolkitAstrodynamicsPy_Trajectory(m); OpenSpaceToolkitAstrodynamicsPy_Flight(m); OpenSpaceToolkitAstrodynamicsPy_Dynamics(m); - OpenSpaceToolkitAstrodynamicsPy_Trajectory(m); OpenSpaceToolkitAstrodynamicsPy_Access(m); OpenSpaceToolkitAstrodynamicsPy_Conjunction(m); OpenSpaceToolkitAstrodynamicsPy_EventCondition(m); diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Profile.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Profile.cpp index 9660423f7..cf40366ed 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Profile.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Flight/Profile.cpp @@ -9,23 +9,192 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Flight_Profile(pybind11::module& aMo using namespace pybind11; using ostk::core::container::Array; + using ostk::core::container::Pair; using ostk::core::type::Shared; + using ostk::mathematics::geometry::d3::transformation::rotation::Quaternion; + using ostk::physics::coordinate::Frame; using ostk::astrodynamics::flight::Profile; using ostk::astrodynamics::flight::profile::Model; + using ostk::astrodynamics::Trajectory; + using ostk::astrodynamics::trajectory::Orbit; using ostk::astrodynamics::trajectory::State; - class_>( + class_> profileClass( aModule, "Profile", R"doc( Spacecraft Flight Profile. + )doc" + ); + + enum_( + profileClass, + "Axis", + R"doc( + The axis of the profile. + )doc" + ) + + .value("X", Profile::Axis::X, "X axis") + .value("Y", Profile::Axis::Y, "Y axis") + .value("Z", Profile::Axis::Z, "Z axis") + + ; + + enum_( + profileClass, + "TargetType", + R"doc( + The target type of the profile. + )doc" + ) + + .value("GeocentricNadir", Profile::TargetType::GeocentricNadir, "Geocentric nadir") + .value("GeodeticNadir", Profile::TargetType::GeodeticNadir, "Geodetic nadir") + .value("Trajectory", Profile::TargetType::Trajectory, "Trajectory") + .value("Sun", Profile::TargetType::Sun, "Sun") + .value("Moon", Profile::TargetType::Moon, "Moon") + .value("VelocityECI", Profile::TargetType::VelocityECI, "Velocity in ECI") + .value("VelocityECEF", Profile::TargetType::VelocityECEF, "Velocity in ECEF") + .value("OrbitalMomentum", Profile::TargetType::OrbitalMomentum, "Orbital momentum") + .value("OrientationProfile", Profile::TargetType::OrientationProfile, "Orientation profile") + + ; + + class_>( + profileClass, + "Target", + R"doc( + The target of the profile. + + )doc" + ) + + .def( + init(), + R"doc( + Constructor. + + Args: + type (Profile.TargetType): The target type. + axis (Profile.Axis): The axis. + anti_direction (bool): True if the direction is flipped, False otherwise. Defaults to False. + )doc", + arg("type"), + arg("axis"), + arg("anti_direction") = false + ) + + .def_readonly("type", &Profile::Target::type, "The type of the target.") + .def_readonly("axis", &Profile::Target::axis, "The axis of the target.") + .def_readonly( + "anti_direction", &Profile::Target::antiDirection, "True if the direction is flipped, False otherwise." + ) + + ; + + class_>( + profileClass, + "TrajectoryTarget", + R"doc( + The trajectory target. + + )doc" + ) + + .def( + init(), + R"doc( + Constructor. + + Args: + trajectory (Trajectory): The trajectory, required only if the target type is `Trajectory`. + axis (Profile.Axis): The axis. + anti_direction (bool): True if the direction is flipped, False otherwise. Defaults to False. + )doc", + arg("trajectory"), + arg("axis"), + arg("anti_direction") = false + ) + + .def_readonly( + "trajectory", + &Profile::TrajectoryTarget::trajectory, + "The trajectory of the target. Required only if the target type is `Trajectory`." + ) + + ; + + class_>( + profileClass, + "OrientationProfileTarget", + R"doc( + The alignment profile target. + + )doc" + ) + + .def( + init>&, const Profile::Axis&, const bool&&>(), + R"doc( + Constructor. + + Args: + 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", + arg("orientation_profile"), + arg("axis"), + arg("anti_direction") = false + ) + + .def_readonly( + "orientation_profile", + &Profile::OrientationProfileTarget::orientationProfile, + "The orientation profile of the target" + ) + + ; + + class_>( + profileClass, + "CustomTarget", + R"doc( + The custom target. + )doc" ) + .def( + init&, const Profile::Axis&, const bool&&>(), + R"doc( + Constructor. + + Args: + orientation_generator (Callable[np.ndarray, State]]): The orientation generator, accepts a state and returns a size 3 array of directions. + axis (Profile.Axis): The axis. + anti_direction (bool): True if the direction is flipped, False otherwise. Defaults to False. + )doc", + arg("orientation_generator"), + arg("axis"), + arg("anti_direction") = false + ) + + .def_readonly( + "orientation_generator", + &Profile::CustomTarget::orientationGenerator, + "The orientation generator of the target" + ) + + ; + + profileClass + .def( init(), R"doc( @@ -156,6 +325,70 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Flight_Profile(pybind11::module& aMo arg("orbital_frame_type") ) + .def_static( + "custom_pointing", + overload_cast&>(&Profile::CustomPointing), + R"doc( + Create a custom pointing profile. + + Args: + orbit (Orbit): The orbit. + orientation_generator (callable[Quaternion, State]): The orientation generator. Typically used in conjunction with `align_and_constrain`. + + Returns: + Profile: The custom pointing profile. + + )doc", + arg("orbit"), + arg("orientation_generator") + ) + + .def_static( + "custom_pointing", + overload_cast< + const Orbit&, + const Shared&, + const Shared&, + const Angle&>(&Profile::CustomPointing), + R"doc( + Create a custom pointing profile. + + Args: + orbit (Orbit): The orbit. + alignment_target (Profile.Target): The alignment target. + clocking_target (Profile.Target): The clocking target. + angular_offset (Angle): The angular offset. Defaults to `Angle.Zero()`. + + Returns: + Profile: The custom pointing profile. + + )doc", + arg("orbit"), + arg("alignment_target"), + arg("clocking_target"), + arg_v("angular_offset", Angle::Zero(), "Angle.Zero()") + ) + + .def_static( + "align_and_constrain", + &Profile::AlignAndConstrain, + R"doc( + Generate a function that provides a quaternion that aligns to the `alignment_target` and constrains to the `clocking_target` for a given state. + + Args: + alignment_target (Profile.Target | Profile.TrajectoryTarget | Profile.OrientationProfileTarget | Profile.CustomTarget): The alignment target. + clocking_target (Profile.Target | Profile.TrajectoryTarget | Profile.OrientationProfileTarget | Profile.CustomTarget): The clocking target. + angular_offset (Angle): The angular offset. Defaults to `Angle.Zero()`. + + Returns: + callable[Quaternion, State]: The custom orientation. + + )doc", + arg("alignment_target"), + arg("clocking_target"), + arg_v("angular_offset", Angle::Zero(), "Angle.Zero()") + ) + ; // Create "profile" python submodule diff --git a/bindings/python/test/flight/test_profile.py b/bindings/python/test/flight/test_profile.py index ddf1de5ae..1c05c34f1 100644 --- a/bindings/python/test/flight/test_profile.py +++ b/bindings/python/test/flight/test_profile.py @@ -8,6 +8,7 @@ from ostk.physics import Environment from ostk.physics.time import DateTime +from ostk.physics.time import Duration from ostk.physics.time import Time from ostk.physics.time import Scale from ostk.physics.time import Instant @@ -32,6 +33,21 @@ def instant() -> Instant: return Instant.date_time(DateTime(2020, 1, 1, 0, 0, 30), Scale.UTC) +@pytest.fixture +def environment() -> Environment: + return Environment.default() + + +@pytest.fixture +def orbit(instant: Instant, environment: Environment) -> Orbit: + return Orbit.sun_synchronous( + epoch=instant, + altitude=Length.kilometers(500.0), + local_time_at_descending_node=Time(14, 0, 0), + celestial_object=environment.access_celestial_object_with_name("Earth"), + ) + + @pytest.fixture def transform_model() -> TransformModel: def dynamic_provider_generator(instant: Instant): @@ -78,11 +94,46 @@ def profile(request) -> Profile: return Profile(model=model) +@pytest.fixture( + params=[ + Profile.Target(Profile.TargetType.GeocentricNadir, Profile.Axis.X), + Profile.TrajectoryTarget( + Trajectory.position(Position.meters((0.0, 0.0, 0.0), Frame.ITRF())), + Profile.Axis.X, + ), + Profile.OrientationProfileTarget( + [ + (Instant.J2000(), [1.0, 0.0, 0.0]), + (Instant.J2000() + Duration.minutes(1.0), [1.0, 0.0, 0.0]), + (Instant.J2000() + Duration.minutes(2.0), [1.0, 0.0, 0.0]), + (Instant.J2000() + Duration.minutes(3.0), [1.0, 0.0, 0.0]), + ], + Profile.Axis.X, + ), + Profile.CustomTarget( + lambda state: [1.0, 0.0, 0.0], + Profile.Axis.X, + ), + ] +) +def alignment_target() -> Profile.Target: + return Profile.Target(Profile.TargetType.GeocentricNadir, Profile.Axis.X) + + +@pytest.fixture +def clocking_target() -> Profile.Target: + return Profile.Target(Profile.TargetType.VelocityECI, Profile.Axis.Y) + + class TestProfile: def test_constructors(self, profile: Profile): assert profile is not None assert isinstance(profile, Profile) + def test_profile_target(self, alignment_target: Profile.Target): + assert alignment_target is not None + assert isinstance(alignment_target, Profile.Target) + def test_get_state_at(self, profile: Profile, instant: Instant): state: State = profile.get_state_at(instant) @@ -130,18 +181,62 @@ def test_inertial_pointing(self): assert isinstance(profile, Profile) assert profile.is_defined() - def test_nadir_pointing(self): - environment = Environment.default() + def test_nadir_pointing( + self, + orbit: Orbit, + ): + profile: Profile = Profile.nadir_pointing(orbit, Orbit.FrameType.VVLH) - orbit = Orbit.sun_synchronous( - epoch=Instant.date_time(datetime(2020, 1, 1, 0, 0, 0), Scale.UTC), - altitude=Length.kilometers(500.0), - local_time_at_descending_node=Time(14, 0, 0), - celestial_object=environment.access_celestial_object_with_name("Earth"), + assert profile is not None + assert isinstance(profile, Profile) + assert profile.is_defined() + + def test_custom_pointing( + self, + orbit: Orbit, + alignment_target: Profile.Target, + clocking_target: Profile.Target, + ): + profile = Profile.custom_pointing( + orbit, Profile.align_and_constrain(alignment_target, clocking_target) ) - profile: Profile = Profile.nadir_pointing(orbit, Orbit.FrameType.VVLH) + assert profile is not None + assert profile.is_defined() + + def test_align_and_constrain( + self, + orbit: Orbit, + instant: Instant, + alignment_target: Profile.Target, + clocking_target: Profile.Target, + ): + orientation = Profile.align_and_constrain( + alignment_target=alignment_target, + clocking_target=clocking_target, + ) + + assert orientation is not None + assert orientation(orbit.get_state_at(instant)) is not None + + def test_custom_pointing( + self, + orbit: Orbit, + alignment_target: Profile.Target, + clocking_target: Profile.Target, + ): + profile = Profile.custom_pointing( + orbit, Profile.align_and_constrain(alignment_target, clocking_target) + ) + + assert profile is not None + assert profile.is_defined() + + profile = Profile.custom_pointing( + orbit, + alignment_target, + clocking_target, + ) assert profile is not None - assert isinstance(profile, Profile) assert profile.is_defined() diff --git a/include/OpenSpaceToolkit/Astrodynamics/Flight/Profile.hpp b/include/OpenSpaceToolkit/Astrodynamics/Flight/Profile.hpp index c77336c75..a3e4570e2 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Flight/Profile.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Flight/Profile.hpp @@ -3,17 +3,26 @@ #ifndef __OpenSpaceToolkit_Astrodynamics_Flight_Profile__ #define __OpenSpaceToolkit_Astrodynamics_Flight_Profile__ +#include + #include +#include +#include +#include #include #include +#include +#include #include #include #include +#include #include #include #include +#include #include #include @@ -28,18 +37,32 @@ namespace flight { using ostk::core::container::Array; +using ostk::core::container::Map; +using ostk::core::container::Pair; +using ostk::core::container::Tuple; +using ostk::core::type::Index; +using ostk::core::type::Integer; +using ostk::core::type::Real; using ostk::core::type::Shared; +using ostk::core::type::Size; using ostk::core::type::String; using ostk::core::type::Unique; +using ostk::mathematics::curvefitting::Interpolator; using ostk::mathematics::geometry::d3::transformation::rotation::Quaternion; +using ostk::mathematics::geometry::d3::transformation::rotation::RotationMatrix; +using ostk::mathematics::object::Matrix3d; +using ostk::mathematics::object::MatrixXd; using ostk::mathematics::object::Vector3d; +using ostk::mathematics::object::VectorXd; using ostk::physics::coordinate::Axes; using ostk::physics::coordinate::Frame; +using ostk::physics::environment::object::Celestial; using ostk::physics::time::Duration; using ostk::physics::time::Instant; using ostk::physics::time::Interval; +using ostk::physics::unit::Angle; using ostk::astrodynamics::flight::profile::Model; using ostk::astrodynamics::Trajectory; @@ -59,6 +82,96 @@ class Profile Custom ///< Custom pointing mode }; + enum class Axis + { + X, + Y, + Z + }; + + enum class TargetType + { + GeocentricNadir, /// Negative of the position vector of the satellite in the ECI frame + GeodeticNadir, /// Negative of the geodetic normal of the satellite in the ECI frame + Trajectory, /// Points towards the provided trajectory, eg. Ground Station in ECEF + Sun, /// The position of the Sun + Moon, /// The position of the Moon + VelocityECI, /// The velocity vector in the ECI frame + VelocityECEF, /// The velocity vector in the ECEF frame + OrbitalMomentum, /// The orbital momentum vector of the satellite in the ECI frame + OrientationProfile, /// Points towards a profile of orientations in the ECI frame + Custom, /// Custom target + }; + + /// @brief Represents a target for alignment or pointing purposes. + class Target + { + public: + /// @brief Constructs a Target object. + /// + /// @param aType The type of the target. + /// @param anAxis The axis of the target. + /// @param isAntiDirection Whether the target is in the anti-direction. + Target(const TargetType& aType, const Axis& anAxis, const bool& isAntiDirection = false); + + TargetType type; ///< The type of the target. + Axis axis; ///< The axis of the target. + bool antiDirection; ///< Whether the target is in the anti-direction. + }; + + /// @brief Represents a target that points towards a trajectory. + class TrajectoryTarget : public Target + { + public: + /// @brief Constructs a TrajectoryTarget object. + /// + /// @param aTrajectory The trajectory to point towards. + /// @param anAxis The axis of the target. + /// @param isAntiDirection Whether the target is in the anti-direction. + TrajectoryTarget(const Trajectory& aTrajectory, const Axis& anAxis, const bool& isAntiDirection = false); + + Trajectory trajectory; ///< The trajectory to point towards. + }; + + /// @brief Represents a target that points towards a profile of orientations. + class OrientationProfileTarget : public Target + { + public: + /// @brief Constructs an OrientationProfileTarget object. + /// + /// @param anOrientationProfile The profile of orientations. + /// @param anAxis The axis of the target. + /// @param isAntiDirection Whether the target is in the anti-direction. + OrientationProfileTarget( + const Array>& anOrientationProfile, + const Axis& anAxis, + const bool& isAntiDirection = false + ); + + /// @brief Gets the alignment vector at a specific instant. + /// + /// @param anInstant The instant at which to get the alignment vector. + /// @return The alignment vector at the specified instant. + Vector3d getAlignmentVectorAt(const Instant& anInstant) const; + + Array> orientationProfile; ///< The profile of orientations. + + private: + Array> interpolators_; ///< Interpolators for the orientation profile. + }; + + class CustomTarget : public Target + { + public: + CustomTarget( + std::function anOrientationGenerator, + const Axis& anAxis, + const bool& isAntiDirection = false + ); + + std::function orientationGenerator; + }; + /// @brief Constructor /// /// @param aModel A model @@ -173,10 +286,68 @@ class Profile const trajectory::Orbit& anOrbit, const trajectory::Orbit::FrameType& anOrbitalFrameType ); + /// @brief Construct a flight profile with custom target pointing + /// + /// @param anOrbit An orbit + /// @param anOrientationGenerator An orientation generator + /// @return Flight profile + static Profile CustomPointing( + const trajectory::Orbit& anOrbit, const std::function& anOrientationGenerator + ); + + /// @brief Construct a flight profile with custom target pointing + /// + /// @param anOrbit An orbit + /// @param anAlignmentTarget An alignment target + /// @param aClockingTarget A clocking target + /// @param anAngularOffset An angular offset + /// @return Flight profile + static Profile CustomPointing( + const trajectory::Orbit& anOrbit, + const Shared& anAlignmentTargetSPtr, + const Shared& aClockingTargetSPtr, + const Angle& anAngularOffset = Angle::Zero() + ); + + /// @brief Generate a function that provides a quaternion that aligns and constrains for a given state. + /// + /// @param anAlignmentAxis An alignment axis + /// @param aClockingAxis A clocking axis + /// @param anAngularOffset An angular offset applied to the clocking axis + + static std::function AlignAndConstrain( + const Shared& anAlignmentTargetSPtr, + const Shared& aClockingTargetSPtr, + const Angle& anAngularOffset = Angle::Zero() + ); + private: Unique modelUPtr_; Profile(); + + static Vector3d ComputeGeocentricNadirDirectionVector(const State& aState); + + static Vector3d ComputeGeodeticNadirDirectionVector(const State& aState); + + static Vector3d ComputeTargetDirectionVector(const State& aState, const Trajectory& aTrajectory); + + static Vector3d ComputeCelestialDirectionVector(const State& aState, const Celestial& aCelestial); + + static Vector3d ComputeVelocityDirectionVector_ECI(const State& aState); + + static Vector3d ComputeVelocityDirectionVector_ECEF(const State& aState); + + static Vector3d ComputeOrbitalMomentumDirectionVector(const State& aState); + + static Vector3d ComputeClockingAxisVector(const Vector3d& anAlignmentAxisVector, const Vector3d& aClockingVector); + + static Quaternion ComputeBodyToECIQuaternion( + const Axis& anAlignmentAxis, + const Axis& aClockingAxis, + const Vector3d& anAlignmentAxisVector, + const Vector3d& aClockingAxisVector + ); }; } // namespace flight diff --git a/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile.cpp b/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile.cpp index d6dc5d8ec..d358fc869 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Flight/Profile.cpp @@ -1,6 +1,11 @@ /// Apache License 2.0 #include +#include +#include +#include +#include +#include #include #include @@ -12,7 +17,91 @@ namespace astrodynamics namespace flight { +using ostk::core::type::Real; + +using ostk::physics::environment::object::celestial::Moon; +using ostk::physics::environment::object::celestial::Sun; +using EarthGravitationalModel = ostk::physics::environment::gravitational::Earth; using ostk::physics::coordinate::frame::Manager; +using ostk::physics::coordinate::Position; +using ostk::physics::coordinate::spherical::LLA; +using ostk::physics::coordinate::Transform; +using ostk::physics::coordinate::Velocity; +using DynamicProvider = ostk::physics::coordinate::frame::provider::Dynamic; + +using TransformModel = ostk::astrodynamics::flight::profile::model::Transform; + +Profile::Target::Target(const TargetType& aType, const Axis& anAxis, const bool& isAntiDirection) + : type(aType), + axis(anAxis), + antiDirection(isAntiDirection) +{ +} + +Profile::TrajectoryTarget::TrajectoryTarget( + const Trajectory& aTrajectory, const Axis& anAxis, const bool& isAntiDirection +) + : Target(TargetType::Trajectory, anAxis, isAntiDirection), + trajectory(aTrajectory) +{ + if (!trajectory.isDefined()) + { + throw ostk::core::error::runtime::Undefined("Trajectory"); + } +} + +Profile::OrientationProfileTarget::OrientationProfileTarget( + const Array>& anOrientationProfile, const Axis& anAxis, const bool& isAntiDirection +) + : Target(TargetType::OrientationProfile, anAxis, isAntiDirection), + orientationProfile(anOrientationProfile) +{ + if (orientationProfile.isEmpty()) + { + throw ostk::core::error::runtime::Undefined("Orientation Profile"); + } + + VectorXd timestamps(anOrientationProfile.getSize()); + MatrixXd coordinates(anOrientationProfile.getSize(), 3); + + for (Index i = 0; i < anOrientationProfile.getSize(); ++i) + { + timestamps(i) = (anOrientationProfile[i].first - anOrientationProfile.accessFirst().first).inSeconds(); + + coordinates.row(i) = anOrientationProfile[i].second; + } + + interpolators_.reserve(coordinates.cols()); + + for (Index i = 0; i < Size(coordinates.cols()); ++i) + { + interpolators_.add( + Interpolator::GenerateInterpolator(Interpolator::Type::BarycentricRational, timestamps, coordinates.col(i)) + ); + } +} + +Vector3d Profile::OrientationProfileTarget::getAlignmentVectorAt(const Instant& anInstant) const +{ + Vector3d interpolatedCoordinates = Vector3d::Zero(); + + const Real duration = (anInstant - orientationProfile.accessFirst().first).inSeconds(); + + for (Index i = 0; i < interpolators_.getSize(); ++i) + { + interpolatedCoordinates(i) = interpolators_[i]->evaluate(duration); + } + + return interpolatedCoordinates; +} + +Profile::CustomTarget::CustomTarget( + std::function anOrientationGenerator, const Axis& anAxis, const bool& isAntiDirection +) + : Target(TargetType::Custom, anAxis, isAntiDirection), + orientationGenerator(anOrientationGenerator) +{ +} Profile::Profile(const Model& aModel) : modelUPtr_(aModel.clone()) @@ -95,7 +184,14 @@ void Profile::print(std::ostream& anOutputStream, bool displayDecorator) const { displayDecorator ? ostk::core::utils::Print::Header(anOutputStream, "Flight Profile") : void(); - // TBI + if (this->isDefined()) + { + this->modelUPtr_->print(anOutputStream, false); + } + else + { + ostk::core::utils::Print::Line(anOutputStream) << "Model:" << "Undefined"; + } displayDecorator ? ostk::core::utils::Print::Footer(anOutputStream) : void(); } @@ -107,16 +203,171 @@ Profile Profile::Undefined() Profile Profile::InertialPointing(const Trajectory& aTrajectory, const Quaternion& aQuaternion) { - using ostk::astrodynamics::flight::profile::model::Transform; - - return {Transform::InertialPointing(aTrajectory, aQuaternion)}; + return {TransformModel::InertialPointing(aTrajectory, aQuaternion)}; } Profile Profile::NadirPointing(const trajectory::Orbit& anOrbit, const trajectory::Orbit::FrameType& anOrbitalFrameType) { - using ostk::astrodynamics::flight::profile::model::Transform; + return {TransformModel::NadirPointing(anOrbit, anOrbitalFrameType)}; +} + +Profile Profile::CustomPointing( + const trajectory::Orbit& anOrbit, + const Shared& anAlignmentTargetSPtr, + const Shared& aClockingTargetSPtr, + const Angle& anAngularOffset +) +{ + const auto orientationGenerator = + Profile::AlignAndConstrain(anAlignmentTargetSPtr, aClockingTargetSPtr, anAngularOffset); + + return Profile::CustomPointing(anOrbit, orientationGenerator); +} + +Profile Profile::CustomPointing( + const trajectory::Orbit& anOrbit, const std::function& anOrientationGenerator +) +{ + // Copy the orientation generator to avoid dangling references. + auto dynamicProviderGenerator = [&anOrbit, anOrientationGenerator](const Instant& anInstant) -> Transform + { + const State state = anOrbit.getStateAt(anInstant); + + const Position position_GCRF = state.getPosition(); + const Velocity velocity_GCRF = state.getVelocity(); + + return Transform::Active( + anInstant, + -position_GCRF.accessCoordinates(), + -velocity_GCRF.accessCoordinates(), + anOrientationGenerator(state), + Vector3d(0.0, 0.0, 0.0) // TBM: Artificially set to 0 for now. + ); + }; + + return Profile(TransformModel(DynamicProvider(dynamicProviderGenerator), Frame::GCRF())); +} + +std::function Profile::AlignAndConstrain( + const Shared& anAlignmentTargetSPtr, + const Shared& aClockingTargetSPtr, + const Angle& anAngularOffset +) +{ + if ((anAlignmentTargetSPtr->type == TargetType::VelocityECEF) || + (aClockingTargetSPtr->type == TargetType::VelocityECEF)) + { + throw ostk::core::error::runtime::ToBeImplemented("Velocity ECEF"); + } + if ((anAlignmentTargetSPtr->type == aClockingTargetSPtr->type) && + (anAlignmentTargetSPtr->type != TargetType::Trajectory)) + { + throw ostk::core::error::RuntimeError("Alignment and clocking target cannot be the same."); + } - return {Transform::NadirPointing(anOrbit, anOrbitalFrameType)}; + if (anAlignmentTargetSPtr->axis == aClockingTargetSPtr->axis) + { + throw ostk::core::error::RuntimeError("Alignment and clocking axis cannot be the same."); + } + + if (std::set({anAlignmentTargetSPtr->type, aClockingTargetSPtr->type}) == + std::set({ + TargetType::GeocentricNadir, + TargetType::GeodeticNadir, + })) + { + throw ostk::core::error::RuntimeError( + "Alignment and clocking target cannot be both geocentric nadir and geodetic nadir." + ); + } + + const auto axisVectorGenerator = [](const Shared& aTargetSPtr + ) -> std::function + { + switch (aTargetSPtr->type) + { + case TargetType::GeocentricNadir: + return Profile::ComputeGeocentricNadirDirectionVector; + case TargetType::GeodeticNadir: + return Profile::ComputeGeodeticNadirDirectionVector; + case TargetType::Trajectory: + { + const Shared trajectoryTargetSPtr = + std::static_pointer_cast(aTargetSPtr); + return [trajectoryTargetSPtr](const State& aState) -> Vector3d + { + return Profile::ComputeTargetDirectionVector(aState, trajectoryTargetSPtr->trajectory); + }; + } + case TargetType::Sun: + return [](const State& aState) + { + return Profile::ComputeCelestialDirectionVector(aState, Sun::Default()); + }; + case TargetType::Moon: + return [](const State& aState) + { + return Profile::ComputeCelestialDirectionVector(aState, Moon::Default()); + }; + case TargetType::VelocityECI: + return Profile::ComputeVelocityDirectionVector_ECI; + case TargetType::VelocityECEF: + return Profile::ComputeVelocityDirectionVector_ECEF; + case TargetType::OrbitalMomentum: + return Profile::ComputeOrbitalMomentumDirectionVector; + case TargetType::OrientationProfile: + { + const Shared orientationProfileTargetSPtr = + std::static_pointer_cast(aTargetSPtr); + return [orientationProfileTargetSPtr](const State& aState) -> Vector3d + { + return orientationProfileTargetSPtr->getAlignmentVectorAt(aState.accessInstant()); + }; + } + case TargetType::Custom: + { + const Shared customTargetSPtr = + std::static_pointer_cast(aTargetSPtr); + return [customTargetSPtr](const State& aState) -> Vector3d + { + return customTargetSPtr->orientationGenerator(aState); + }; + } + default: + throw ostk::core::error::RuntimeError("Invalid alignment target type."); + } + }; + + const auto alignmentAxisVectorFunction = axisVectorGenerator(anAlignmentTargetSPtr); + const auto clockingAxisVectorFunction = axisVectorGenerator(aClockingTargetSPtr); + + const Integer alignmentSign = anAlignmentTargetSPtr->antiDirection ? -1 : 1; + const Integer clockingSign = aClockingTargetSPtr->antiDirection ? -1 : 1; + + return [anAngularOffset, + alignmentAxisVectorFunction, + clockingAxisVectorFunction, + anAlignmentTargetSPtr, + aClockingTargetSPtr, + alignmentSign, + clockingSign](const State& aState) -> Quaternion + { + const Vector3d alignemntAxisVector = alignmentAxisVectorFunction(aState) * alignmentSign; + const Vector3d clockingVector = clockingAxisVectorFunction(aState) * clockingSign; + + const Vector3d clockingAxisVector = Profile::ComputeClockingAxisVector(alignemntAxisVector, clockingVector); + + const Real thetaOffsetRad = anAngularOffset.inRadians(); + + const Vector3d rotatedClockingAxisVector = + (clockingAxisVector * std::cos(thetaOffsetRad) + + (alignemntAxisVector.cross(clockingAxisVector)) * std::sin(thetaOffsetRad) + + alignemntAxisVector * (alignemntAxisVector.dot(clockingAxisVector)) * (1.0 - std::cos(thetaOffsetRad))); + + return Profile::ComputeBodyToECIQuaternion( + anAlignmentTargetSPtr->axis, aClockingTargetSPtr->axis, alignemntAxisVector, rotatedClockingAxisVector + ); + }; } Profile::Profile() @@ -124,6 +375,122 @@ Profile::Profile() { } +Vector3d Profile::ComputeGeocentricNadirDirectionVector(const State& aState) +{ + return -aState.getPosition().accessCoordinates().normalized(); +} + +Vector3d Profile::ComputeGeodeticNadirDirectionVector(const State& aState) +{ + const Transform ITRF_GCRF_transform = Frame::ITRF()->getTransformTo(Frame::GCRF(), aState.accessInstant()); + + const LLA lla = LLA::Cartesian( + ITRF_GCRF_transform.getInverse().applyToPosition(aState.getPosition().accessCoordinates()), + EarthGravitationalModel::EGM2008.equatorialRadius_, + EarthGravitationalModel::EGM2008.flattening_ + ); + + const Vector3d nadir = { + -std::cos(lla.getLatitude().inRadians()) * std::cos(lla.getLongitude().inRadians()), + -std::cos(lla.getLatitude().inRadians()) * std::sin(lla.getLongitude().inRadians()), + -std::sin(lla.getLatitude().inRadians()), + }; + + return ITRF_GCRF_transform.applyToVector(nadir).normalized(); +} + +Vector3d Profile::ComputeTargetDirectionVector(const State& aState, const Trajectory& aTrajectory) +{ + if (!aTrajectory.isDefined()) + { + throw ostk::core::error::runtime::Undefined("Trajectory"); + } + + const Vector3d targetPositionCoordinates = + aTrajectory.getStateAt(aState.accessInstant()).getPosition().accessCoordinates(); + const Vector3d satellitePositionCoordinates = aState.getPosition().accessCoordinates(); + + return (targetPositionCoordinates - satellitePositionCoordinates).normalized(); +} + +Vector3d Profile::ComputeCelestialDirectionVector(const State& aState, const Celestial& aCelestial) +{ + const Vector3d celestialPositionCoordinates = + aCelestial.getPositionIn(Frame::GCRF(), aState.getInstant()).accessCoordinates(); + const Vector3d satellitePositionCoordinates = aState.getPosition().accessCoordinates(); + + return (celestialPositionCoordinates - satellitePositionCoordinates).normalized(); +} + +Vector3d Profile::ComputeVelocityDirectionVector_ECI(const State& aState) +{ + return aState.getVelocity().accessCoordinates().normalized(); +} + +Vector3d Profile::ComputeVelocityDirectionVector_ECEF(const State& aState) +{ + return aState.inFrame(Frame::ITRF()).getVelocity().accessCoordinates().normalized(); +} + +Vector3d Profile::ComputeClockingAxisVector(const Vector3d& anAlignmentAxisVector, const Vector3d& aClockingVector) +{ + return (anAlignmentAxisVector.cross(aClockingVector)).cross(anAlignmentAxisVector).normalized(); +} + +Vector3d Profile::ComputeOrbitalMomentumDirectionVector(const State& aState) +{ + const Vector3d positionDirection = aState.getPosition().getCoordinates().normalized(); + const Vector3d velocityDirection = aState.getVelocity().getCoordinates().normalized(); + + return positionDirection.cross(velocityDirection).normalized(); +} + +Quaternion Profile::ComputeBodyToECIQuaternion( + const Axis& anAlignmentAxis, + const Axis& aClockingAxis, + const Vector3d& anAlignmentAxisVector, + const Vector3d& aClockingAxisVector +) +{ + static const Map axisIndexMap = { + {Axis::X, 0}, + {Axis::Y, 1}, + {Axis::Z, 2}, + }; + + static const Map, Tuple> triadAxisIndexMap = { + {{Axis::X, Axis::Y}, {0, 1}}, + {{Axis::X, Axis::Z}, {2, 0}}, + {{Axis::Y, Axis::X}, {0, 1}}, + {{Axis::Y, Axis::Z}, {1, 2}}, + {{Axis::Z, Axis::X}, {2, 0}}, + {{Axis::Z, Axis::Y}, {1, 2}}, + }; + static const std::set allAxes = {Axis::X, Axis::Y, Axis::Z}; + + const std::set usedAxes = {anAlignmentAxis, aClockingAxis}; + + std::vector difference; + std::set_difference( + allAxes.begin(), allAxes.end(), usedAxes.begin(), usedAxes.end(), std::inserter(difference, difference.begin()) + ); + const Axis triadAxis = difference.front(); + + Integer firstAxisVectorIndex = Integer::Undefined(); + Integer secondAxisVectorIndex = Integer::Undefined(); + std::tie(firstAxisVectorIndex, secondAxisVectorIndex) = + triadAxisIndexMap.at(Tuple {anAlignmentAxis, aClockingAxis}); + + Matrix3d rotationMatrix = Matrix3d::Zero(); + + rotationMatrix.row(axisIndexMap.at(anAlignmentAxis)) = anAlignmentAxisVector; + rotationMatrix.row(axisIndexMap.at(aClockingAxis)) = aClockingAxisVector; + rotationMatrix.row(axisIndexMap.at(triadAxis)) = + rotationMatrix.row(firstAxisVectorIndex).cross(rotationMatrix.row(secondAxisVectorIndex)); + + return Quaternion::RotationMatrix(rotationMatrix); +} + } // namespace flight } // namespace astrodynamics } // namespace ostk diff --git a/test/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.test.cpp index 69399a450..383c43855 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Flight/Maneuver.test.cpp @@ -56,8 +56,8 @@ class OpenSpaceToolkit_Astrodynamics_Flight_Maneuver : public ::testing::Test const Array defaultInstants_ = { Instant::J2000(), Instant::J2000() + Duration::Seconds(1.0), - Instant::J2000() + Duration::Seconds(5.0), - Instant::J2000() + Duration::Seconds(7.0), + Instant::J2000() + Duration::Seconds(15.0), + Instant::J2000() + Duration::Seconds(37.0), }; const Array defaultAccelerationProfileDefaultFrame_ = { diff --git a/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp index 43dd687ab..40fb71c00 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Flight/Profile.test.cpp @@ -1,5 +1,7 @@ /// Apache License 2.0 +#include + #include #include #include @@ -8,8 +10,11 @@ #include #include +#include #include #include +#include +#include #include #include #include @@ -29,6 +34,8 @@ #include using ostk::core::container::Array; +using ostk::core::container::Map; +using ostk::core::container::Pair; using ostk::core::container::Table; using ostk::core::filesystem::File; using ostk::core::filesystem::Path; @@ -42,14 +49,19 @@ using ostk::mathematics::object::Vector3d; using ostk::physics::coordinate::Frame; using ostk::physics::coordinate::Position; +using ostk::physics::coordinate::spherical::LLA; using ostk::physics::coordinate::Velocity; using ostk::physics::Environment; -using ostk::physics::environment::gravitational::Earth; +using ostk::physics::environment::object::celestial::Earth; +using ostk::physics::environment::object::celestial::Moon; +using ostk::physics::environment::object::celestial::Sun; +using EarthGravitationalModel = ostk::physics::environment::gravitational::Earth; using ostk::physics::time::DateTime; using ostk::physics::time::Duration; using ostk::physics::time::Instant; using ostk::physics::time::Interval; using ostk::physics::time::Scale; +using ostk::physics::time::Time; using ostk::physics::unit::Angle; using ostk::physics::unit::Derived; using ostk::physics::unit::Length; @@ -57,6 +69,7 @@ using ostk::physics::unit::Length; using ostk::astrodynamics::flight::Profile; using ostk::astrodynamics::flight::profile::model::Tabulated; using ostk::astrodynamics::flight::profile::model::Transform; +using ostk::astrodynamics::Trajectory; using ostk::astrodynamics::trajectory::Orbit; using ostk::astrodynamics::trajectory::orbit::model::Kepler; using ostk::astrodynamics::trajectory::orbit::model::kepler::COE; @@ -79,10 +92,10 @@ class OpenSpaceToolkit_Astrodynamics_Flight_Profile : public ::testing::Test const COE coe = {semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly}; const Instant epoch = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); - const Derived gravitationalParameter = Earth::EGM2008.gravitationalParameter_; - const Length equatorialRadius = Earth::EGM2008.equatorialRadius_; - const Real J2 = Earth::EGM2008.J2_; - const Real J4 = Earth::EGM2008.J4_; + const Derived gravitationalParameter = EarthGravitationalModel::EGM2008.gravitationalParameter_; + const Length equatorialRadius = EarthGravitationalModel::EGM2008.equatorialRadius_; + const Real J2 = EarthGravitationalModel::EGM2008.J2_; + const Real J4 = EarthGravitationalModel::EGM2008.J4_; const Kepler keplerianModel = { coe, epoch, gravitationalParameter, equatorialRadius, J2, J4, Kepler::PerturbationType::None @@ -127,6 +140,14 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, StreamOperator) EXPECT_FALSE(testing::internal::GetCapturedStdout().empty()); } + + { + testing::internal::CaptureStdout(); + + EXPECT_NO_THROW(std::cout << Profile::Undefined() << std::endl); + + EXPECT_FALSE(testing::internal::GetCapturedStdout().empty()); + } } TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, IsDefined) @@ -253,10 +274,10 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, InertialPointing) const COE coe = {semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly}; const Instant epoch = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); - const Derived gravitationalParameter = Earth::EGM2008.gravitationalParameter_; - const Length equatorialRadius = Earth::EGM2008.equatorialRadius_; - const Real J2 = Earth::EGM2008.J2_; - const Real J4 = Earth::EGM2008.J4_; + const Derived gravitationalParameter = EarthGravitationalModel::EGM2008.gravitationalParameter_; + const Length equatorialRadius = EarthGravitationalModel::EGM2008.equatorialRadius_; + const Real J2 = EarthGravitationalModel::EGM2008.J2_; + const Real J4 = EarthGravitationalModel::EGM2008.J4_; const Kepler keplerianModel = { coe, epoch, gravitationalParameter, equatorialRadius, J2, J4, Kepler::PerturbationType::None @@ -366,10 +387,10 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing_VVLH) const COE coe = {semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly}; const Instant epoch = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); - const Derived gravitationalParameter = Earth::EGM2008.gravitationalParameter_; - const Length equatorialRadius = Earth::EGM2008.equatorialRadius_; - const Real J2 = Earth::EGM2008.J2_; - const Real J4 = Earth::EGM2008.J4_; + const Derived gravitationalParameter = EarthGravitationalModel::EGM2008.gravitationalParameter_; + const Length equatorialRadius = EarthGravitationalModel::EGM2008.equatorialRadius_; + const Real J2 = EarthGravitationalModel::EGM2008.J2_; + const Real J4 = EarthGravitationalModel::EGM2008.J4_; const Kepler keplerianModel = { coe, epoch, gravitationalParameter, equatorialRadius, J2, J4, Kepler::PerturbationType::None @@ -476,10 +497,10 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing_VVLH) const COE coe = {semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly}; const Instant epoch = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); - const Derived gravitationalParameter = Earth::EGM2008.gravitationalParameter_; - const Length equatorialRadius = Earth::EGM2008.equatorialRadius_; - const Real J2 = Earth::EGM2008.J2_; - const Real J4 = Earth::EGM2008.J4_; + const Derived gravitationalParameter = EarthGravitationalModel::EGM2008.gravitationalParameter_; + const Length equatorialRadius = EarthGravitationalModel::EGM2008.equatorialRadius_; + const Real J2 = EarthGravitationalModel::EGM2008.J2_; + const Real J4 = EarthGravitationalModel::EGM2008.J4_; const Kepler keplerianModel = { coe, epoch, gravitationalParameter, equatorialRadius, J2, J4, Kepler::PerturbationType::None @@ -586,10 +607,10 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, NadirPointing_VVLH) const COE coe = {semiMajorAxis, eccentricity, inclination, raan, aop, trueAnomaly}; const Instant epoch = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); - const Derived gravitationalParameter = Earth::EGM2008.gravitationalParameter_; - const Length equatorialRadius = Earth::EGM2008.equatorialRadius_; - const Real J2 = Earth::EGM2008.J2_; - const Real J4 = Earth::EGM2008.J4_; + const Derived gravitationalParameter = EarthGravitationalModel::EGM2008.gravitationalParameter_; + const Length equatorialRadius = EarthGravitationalModel::EGM2008.equatorialRadius_; + const Real J2 = EarthGravitationalModel::EGM2008.J2_; + const Real J4 = EarthGravitationalModel::EGM2008.J4_; const Kepler keplerianModel = { coe, epoch, gravitationalParameter, equatorialRadius, J2, J4, Kepler::PerturbationType::None @@ -788,3 +809,401 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, Tabulated) } } } + +TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, Target) +{ + { + EXPECT_NO_THROW(Profile::Target(Profile::TargetType::GeocentricNadir, Profile::Axis::X)); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, TrajectoryTarget) +{ + { + EXPECT_THROW( + Profile::TrajectoryTarget(Trajectory::Undefined(), Profile::Axis::X), ostk::core::error::runtime::Undefined + ); + } + + { + const Trajectory trajectory = Trajectory::Position(Position::Meters({0.0, 0.0, 0.0}, Frame::ITRF())); + EXPECT_NO_THROW(Profile::TrajectoryTarget(trajectory, Profile::Axis::X)); + EXPECT_NO_THROW(Profile::TrajectoryTarget(trajectory, Profile::Axis::X, true)); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, CustomTarget) +{ + { + std::function orientationGenerator = [](const State&) -> Vector3d + { + return Vector3d::X(); + }; + + { + EXPECT_NO_THROW(Profile::CustomTarget(orientationGenerator, Profile::Axis::X)); + } + + { + EXPECT_NO_THROW(Profile::CustomTarget(orientationGenerator, Profile::Axis::X, true)); + } + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, OrientationProfileTarget) +{ + { + EXPECT_THROW(Profile::OrientationProfileTarget({}, Profile::Axis::X), ostk::core::error::runtime::Undefined); + } + + { + const Array> orientationProfile = { + {Instant::J2000(), Vector3d::X()}, + {Instant::J2000() + Duration::Seconds(10.0), Vector3d::X()}, + {Instant::J2000() + Duration::Seconds(20.0), Vector3d::X()}, + {Instant::J2000() + Duration::Seconds(30.0), Vector3d::X()}, + {Instant::J2000() + Duration::Seconds(.0), Vector3d::X()}, + }; + EXPECT_NO_THROW(Profile::OrientationProfileTarget(orientationProfile, Profile::Axis::X)); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, AlignAndConstrain) +{ + { + EXPECT_THROW( + Profile::AlignAndConstrain( + std::make_shared(Profile::TargetType::VelocityECEF, Profile::Axis::X), + std::make_shared(Profile::TargetType::GeocentricNadir, Profile::Axis::Y) + ), + ostk::core::error::runtime::ToBeImplemented + ); + } + + { + EXPECT_THROW( + Profile::AlignAndConstrain( + std::make_shared(Profile::TargetType::GeocentricNadir, Profile::Axis::Y), + std::make_shared(Profile::TargetType::VelocityECEF, Profile::Axis::X) + ), + ostk::core::error::runtime::ToBeImplemented + ); + } + + { + const Instant epoch = Instant::J2000(); + + const Shared earthSPtr = std::make_shared(Earth::Default()); + + const Orbit orbit = Orbit::SunSynchronous(epoch, Length::Kilometers(500.0), Time(6, 0, 0), earthSPtr); + + const State state = orbit.getStateAt(epoch); + + // Trajectory + { + const Trajectory trajectory = Trajectory::Position(Position::Meters({0.0, 0.0, 0.0}, Frame::ITRF())); + const auto orientation = Profile::AlignAndConstrain( + std::make_shared(trajectory, Profile::Axis::X, false), + std::make_shared(Profile::TargetType::VelocityECI, Profile::Axis::Y) + ); + + const Quaternion q_B_GCRF = orientation(state); + + EXPECT_VECTORS_ALMOST_EQUAL( + q_B_GCRF * -state.getPosition().getCoordinates().normalized(), Vector3d::X(), 1e-12 + ); + } + + // Orientation profile + { + const Array> orientationProfile = { + {Instant::J2000(), Vector3d::X()}, + {Instant::J2000() + Duration::Seconds(10.0), Vector3d::X()}, + {Instant::J2000() + Duration::Seconds(20.0), Vector3d::X()}, + {Instant::J2000() + Duration::Seconds(30.0), Vector3d::X()}, + {Instant::J2000() + Duration::Seconds(40.0), Vector3d::X()}, + }; + + const auto orientation = Profile::AlignAndConstrain( + std::make_shared(orientationProfile, Profile::Axis::X), + std::make_shared(Profile::TargetType::VelocityECI, Profile::Axis::Y) + ); + + const Quaternion q_B_GCRF = orientation(state); + + EXPECT_VECTORS_ALMOST_EQUAL(q_B_GCRF * Vector3d::X(), Vector3d::X(), 1e-12); + } + + // Custom profile + { + std::function orientationGenerator = [](const State&) -> Vector3d + { + return Vector3d::X(); + }; + + const auto orientation = Profile::AlignAndConstrain( + std::make_shared(orientationGenerator, Profile::Axis::X, false), + std::make_shared(Profile::TargetType::VelocityECI, Profile::Axis::Y) + ); + + const Quaternion q_B_GCRF = orientation(state); + + EXPECT_VECTORS_ALMOST_EQUAL(q_B_GCRF * Vector3d::X(), Vector3d::X(), 1e-12); + } + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Flight_Profile, CustomPointing) +{ + const Instant epoch = Instant::J2000(); + + const Shared earthSPtr = std::make_shared(Earth::Default()); + + const Orbit orbit = Orbit::SunSynchronous(epoch, Length::Kilometers(500.0), Time(6, 0, 0), earthSPtr); + + const Shared alignmentTargetSPtr = + std::make_shared(Profile::TargetType::VelocityECI, Profile::Axis::X); + + const Shared clockingTargetSPtr = + std::make_shared(Profile::TargetType::OrbitalMomentum, Profile::Axis::Y); + + { + // VNC frame + + const auto orientation = Profile::AlignAndConstrain(alignmentTargetSPtr, clockingTargetSPtr); + + const Profile calculatedProfile = Profile::CustomPointing(orbit, orientation); + + // Compare against Orbit VNC frame which has been validated + + const Profile expectedProfile = Profile::NadirPointing(orbit, Orbit::FrameType::VNC); + + for (const auto instant : + Interval::Closed(epoch, epoch + Duration::Hours(1.0)).generateGrid(Duration::Minutes(5.0))) + { + const State calculatedState = calculatedProfile.getStateAt(instant); + const State expectedState = expectedProfile.getStateAt(instant); + + EXPECT_VECTORS_ALMOST_EQUAL( + calculatedState.getPosition().getCoordinates(), expectedState.getPosition().getCoordinates(), 1e-6 + ); + EXPECT_VECTORS_ALMOST_EQUAL( + calculatedState.getVelocity().getCoordinates(), expectedState.getVelocity().getCoordinates(), 1e-6 + ); + EXPECT_TRUE(calculatedState.getAttitude().isNear(expectedState.getAttitude(), Angle::Arcseconds(1e-12))); + } + } + + // Self consistent test for the interface function + { + const auto orientation = Profile::AlignAndConstrain(alignmentTargetSPtr, clockingTargetSPtr); + + const Profile expectedProfile = Profile::CustomPointing(orbit, orientation); + + const Profile calculatedProfile = Profile::CustomPointing(orbit, alignmentTargetSPtr, clockingTargetSPtr); + + for (const auto instant : + Interval::Closed(epoch, epoch + Duration::Hours(1.0)).generateGrid(Duration::Minutes(5.0))) + { + const State calculatedState = calculatedProfile.getStateAt(instant); + const State expectedState = expectedProfile.getStateAt(instant); + + EXPECT_VECTORS_ALMOST_EQUAL( + calculatedState.getPosition().getCoordinates(), expectedState.getPosition().getCoordinates(), 1e-15 + ); + EXPECT_VECTORS_ALMOST_EQUAL( + calculatedState.getVelocity().getCoordinates(), expectedState.getVelocity().getCoordinates(), 1e-15 + ); + EXPECT_TRUE(calculatedState.getAttitude().isNear(expectedState.getAttitude(), Angle::Arcseconds(1e-15))); + } + } +} + +class OpenSpaceToolkit_Astrodynamics_Flight_Profile_Parametrized + : public ::testing::TestWithParam< + std::tuple> +{ +}; + +TEST_P(OpenSpaceToolkit_Astrodynamics_Flight_Profile_Parametrized, AlignAndConstrain) +{ + const auto param = GetParam(); + Profile::TargetType alignmentTargetType; + Profile::TargetType clockingTargetType; + Profile::Axis alignmentAxis; + Profile::Axis clockingAxis; + std::tie(alignmentAxis, clockingAxis, alignmentTargetType, clockingTargetType) = param; + + const Shared alignmentTargetSPtr = + std::make_shared(alignmentTargetType, alignmentAxis); + + const Shared clockingTargetSPtr = + std::make_shared(clockingTargetType, clockingAxis); + + // check failure conditions + + if (alignmentAxis == clockingAxis) + { + EXPECT_THROW( + Profile::AlignAndConstrain(alignmentTargetSPtr, clockingTargetSPtr), ostk::core::error::RuntimeError + ); + return; + } + + if (alignmentTargetType == clockingTargetType) + { + EXPECT_THROW( + Profile::AlignAndConstrain(alignmentTargetSPtr, clockingTargetSPtr), ostk::core::error::RuntimeError + ); + return; + } + + if (((alignmentTargetType == Profile::TargetType::GeocentricNadir) && + (clockingTargetType == Profile::TargetType::GeodeticNadir)) || + ((clockingTargetType == Profile::TargetType::GeocentricNadir) && + (alignmentTargetType == Profile::TargetType::GeodeticNadir))) + { + EXPECT_THROW( + Profile::AlignAndConstrain(alignmentTargetSPtr, clockingTargetSPtr), ostk::core::error::RuntimeError + ); + return; + } + + const Instant epoch = Instant::J2000(); + + const Shared earthSPtr = std::make_shared(Earth::Default()); + + const Orbit orbit = Orbit::SunSynchronous(epoch, Length::Kilometers(500.0), Time(6, 0, 0), earthSPtr); + + const State state = orbit.getStateAt(epoch); + + static const Map axisMap = { + {Profile::Axis::X, {1.0, 0.0, 0.0}}, + {Profile::Axis::Y, {0.0, 1.0, 0.0}}, + {Profile::Axis::Z, {0.0, 0.0, 1.0}}, + }; + + const std::function orientation = + Profile::AlignAndConstrain(alignmentTargetSPtr, clockingTargetSPtr); + + const Quaternion q_B_GCRF = orientation(state); + + const auto getExpectedDirectionVector = + [&earthSPtr](const Profile::TargetType& aTargetType, const State& aState) -> Vector3d + { + switch (aTargetType) + { + case Profile::TargetType::GeocentricNadir: + return -aState.getPosition().getCoordinates().normalized(); + break; + + case Profile::TargetType::GeodeticNadir: + { + const LLA lla = LLA::Cartesian( + aState.inFrame(Frame::ITRF()).getPosition().getCoordinates(), + earthSPtr->getEquatorialRadius(), + earthSPtr->getFlattening() + ); + const LLA llaOnSurface = LLA(lla.getLatitude(), lla.getLongitude(), Length::Meters(0.0)); + Vector3d positionOnSurface = + Position::Meters( + llaOnSurface.toCartesian(earthSPtr->getEquatorialRadius(), earthSPtr->getFlattening()), + Frame::ITRF() + ) + .inFrame(Frame::GCRF(), aState.accessInstant()) + .getCoordinates(); + return (positionOnSurface - aState.getPosition().getCoordinates()).normalized(); + } + break; + + case Profile::TargetType::VelocityECI: + return aState.getVelocity().getCoordinates().normalized(); + break; + + case Profile::TargetType::Sun: + { + const Sun sun = Sun::Default(); + return (sun.getPositionIn(Frame::GCRF(), aState.accessInstant()).getCoordinates() - + aState.getPosition().getCoordinates()) + .normalized(); + } + break; + + case Profile::TargetType::Moon: + { + const Moon moon = Moon::Default(); + return (moon.getPositionIn(Frame::GCRF(), aState.accessInstant()).getCoordinates() - + aState.getPosition().getCoordinates()) + .normalized(); + } + break; + + default: + return Vector3d::Zero(); + break; + } + }; + + // check alignment axis + + const Vector3d alignmentAxisVector = axisMap.at(alignmentAxis); + + const Vector3d estimatedAlignmentDirection = (q_B_GCRF.toConjugate() * alignmentAxisVector); + + const Vector3d expectedAlignmentDirection = getExpectedDirectionVector(alignmentTargetType, state); + + EXPECT_VECTORS_ALMOST_EQUAL(expectedAlignmentDirection, estimatedAlignmentDirection, 1e-12); + + // check clocking axis + + const Vector3d clockingAxisVector = axisMap.at(clockingAxis); + + const Vector3d estimatedClockingDirection = (q_B_GCRF.toConjugate() * clockingAxisVector); + + const Vector3d expectedClockingDirection = getExpectedDirectionVector(clockingTargetType, state); + + // TBI: These tests can be improved in the future + if (alignmentTargetType == Profile::TargetType::VelocityECI && clockingTargetType == Profile::TargetType::Sun) + { + EXPECT_LT(Angle::Between(expectedClockingDirection, estimatedClockingDirection).inDegrees(), 16.0); + } + else if (clockingTargetType == Profile::TargetType::Sun) + { + EXPECT_LT(Angle::Between(expectedClockingDirection, estimatedClockingDirection).inDegrees(), 1.0); + } + else + { + EXPECT_LT(Angle::Between(expectedClockingDirection, estimatedClockingDirection).inDegrees(), 1e-3); + } +} + +std::string ParamNameGenerator( + const ::testing::TestParamInfo>& + info +) +{ + std::string name = "Axis" + std::to_string(static_cast(std::get<0>(info.param))) + "_Axis" + + std::to_string(static_cast(std::get<1>(info.param))) + "_Target" + + std::to_string(static_cast(std::get<2>(info.param))) + "_Target" + + std::to_string(static_cast(std::get<3>(info.param))); + return name; +} + +INSTANTIATE_TEST_SUITE_P( + OpenSpaceToolkit_Astrodynamics_Flight_Profile_Parametrized_Values, + OpenSpaceToolkit_Astrodynamics_Flight_Profile_Parametrized, + ::testing::Combine( + ::testing::Values(Profile::Axis::X), + ::testing::Values(Profile::Axis::Y), + ::testing::Values( + Profile::TargetType::GeocentricNadir, Profile::TargetType::GeodeticNadir, Profile::TargetType::VelocityECI + ), + ::testing::Values( + Profile::TargetType::GeocentricNadir, + Profile::TargetType::GeodeticNadir, + Profile::TargetType::VelocityECI, + Profile::TargetType::Sun + ) + ), + ParamNameGenerator +);