Skip to content

Commit

Permalink
feat: add periapsis scaling for QLaw and fix small bugs in oe_xx calc…
Browse files Browse the repository at this point in the history 
…ulations (#270)

* feat: add periapsis scaling for QLaw and fix some small bugs

* feat: works well end to end

* feat: fix style

* feat: fix tests

* feat: add more tests

* chore: remove stdcout

* feat: use Eigen map
  • Loading branch information
@vishwa2710
vishwa2710 authored Nov 14, 2023
1 parent 92c1251 commit f477d29
Show file tree
Hide file tree
Showing 7 changed files with 492 additions and 121 deletions.
59 changes: 45 additions & 14 deletions bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/GuidanceLaw/QLaw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
using namespace pybind11;

using ostk::core::types::String;
using ostk::core::types::Real;
using ostk::core::types::Shared;
using ostk::core::types::Size;
using ostk::core::ctnr::Map;
Expand Down Expand Up @@ -37,7 +36,7 @@ void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_QLaw(pybind11::module& aModule)
"m",
&QLaw::Parameters::m,
R"doc(
Scaling parameter for SMA delta.
Scaling parameter for Semi-Major Axis delta.
Type:
int
Expand All @@ -47,7 +46,7 @@ void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_QLaw(pybind11::module& aModule)
"n",
&QLaw::Parameters::n,
R"doc(
Scaling parameter for SMA delta.
Scaling parameter for Semi-Major Axis delta.
Type:
int
Expand All @@ -57,7 +56,17 @@ void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_QLaw(pybind11::module& aModule)
"r",
&QLaw::Parameters::r,
R"doc(
Scaling parameter for SMA delta.
Scaling parameter for Semi-Major Axis delta.
Type:
int
)doc"
)
.def_readonly(
"k",
&QLaw::Parameters::r,
R"doc(
Penalty parameter for periapsis.
Type:
int
Expand All @@ -67,31 +76,42 @@ void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_QLaw(pybind11::module& aModule)
"b",
&QLaw::Parameters::b,
R"doc(
Scaling parameter for SMA delta.
Scaling parameter for Argument of Periapsis.
Type:
float
)doc"
)

.def(
init<const Map<COE::Element, Real>&, const Size&, const Size&, const Size&, const Real&>(),
init<
const Map<COE::Element, double>&,
const Size&,
const Size&,
const Size&,
const Size&,
const Length&,
const double&>(),
R"doc(
Constructor.
Args:
element_weights (dict): Key-value pair of COE elements and the weights for the targeter.
m (int): Scaling parameter for SMA delta.
n (int): Scaling parameter for SMA delta.
r (int): Scaling parameter for SMA delta.
b (float): Scaling parameter for SMA delta.
m (int): Scaling parameter for Semi-Major Axis delta. Default to 3.
n (int): Scaling parameter for Semi-Major Axis delta. Default to 4.
r (int): Scaling parameter for Semi-Major Axis delta. Default to 2.
k (int): Penalty parameter for periapsis. Default to 100.
minimum_periapsis_radius (Length): Minimum periapsis radius. Default to 6578.0 km.
b (float): Scaling parameter for Argument of Periapsis maximal change. Default to 0.01.
)doc",
arg("element_weights"),
arg("m"),
arg("n"),
arg("r"),
arg("b")
arg("m") = 3,
arg("n") = 4,
arg("r") = 2,
arg("k") = 100,
arg("minimum_periapsis_radius") = Length::Kilometers(6578.0),
arg("b") = 0.01
)

.def(
Expand All @@ -105,6 +125,17 @@ void OpenSpaceToolkitAstrodynamicsPy_GuidanceLaw_QLaw(pybind11::module& aModule)
)doc"
)

.def(
"get_minimum_periapsis_radius",
&QLaw::Parameters::getMinimumPeriapsisRadius,
R"doc(
Get the minimum periapsis radius.
Returns:
Length: The minimum periapsis radius.
)doc"
)

;

class_<QLaw, GuidanceLaw, Shared<QLaw>>(
Expand Down
4 changes: 0 additions & 4 deletions bindings/python/test/guidance_law/test_qlaw.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,6 @@ def gravitational_parameter() -> Derived:
def parameters() -> QLawParameters:
return QLawParameters(
element_weights={COE.Element.SemiMajorAxis: 1.0, COE.Element.Eccentricity: 1.0},
m=3,
n=4,
r=2,
b=0.01,
)


Expand Down
5 changes: 5 additions & 0 deletions bindings/python/tools/python/setup.cfg.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,11 @@ classifiers =
zip_safe = True
include_package_data = True
install_requires = file: requirements.txt
packages = find:

[options.packages.find]
include = ostk.*, pytrajectory.*
exclude = ostk.astrodynamics.test.*

[options.package_data]
ostk.astrodynamics=${SHARED_LIBRARY_TARGET}.${PROJECT_VERSION_MAJOR}, ${LIBRARY_TARGET}.*${EXTENSION}*.so
Expand Down
82 changes: 67 additions & 15 deletions include/OpenSpaceToolkit/Astrodynamics/GuidanceLaw/QLaw.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
#define __OpenSpaceToolkit_Astrodynamics_GuidanceLaw_QLaw__

#include <OpenSpaceToolkit/Core/Containers/Map.hpp>
#include <OpenSpaceToolkit/Core/Types/Real.hpp>

#include <OpenSpaceToolkit/Mathematics/Objects/Vector.hpp>

Expand Down Expand Up @@ -39,6 +38,7 @@ using ostk::math::obj::MatrixXd;
using ostk::physics::time::Instant;
using ostk::physics::coord::Frame;
using ostk::physics::units::Derived;
using ostk::physics::units::Length;

using ostk::astro::trajectory::orbit::models::kepler::COE;
using ostk::astro::GuidanceLaw;
Expand All @@ -61,21 +61,29 @@ class QLaw : public GuidanceLaw
struct Parameters
{
Parameters(
const Map<COE::Element, Real>& anElementWeights,
const Map<COE::Element, double>& anElementWeights,
const Size& aMValue = 3,
const Size& aNValue = 4,
const Size& aRValue = 2,
const Real& aBValue = 0.01
const Size& aKValue = 100,
const Length& minimumPeriapsisradius = Length::Kilometers(6578.0),
const double& aBValue = 0.01
);

Vector5d getControlWeights() const;
Length getMinimumPeriapsisRadius() const;

const Size m;
const Size n;
const Size r;
const Real b;
const double k;
const double b;
const double periapsisWeight = 1.0;

friend QLaw;

private:
const double minimumPeriapsisRadius_;
Vector5d controlWeights_ = Vector5d::Zero();
};

Expand Down Expand Up @@ -147,23 +155,67 @@ class QLaw : public GuidanceLaw
const Shared<const Frame>& outputFrameSPtr
) const override;

/// @brief Compute the maximal change in orbital elements
///
/// @param [in] aCOEVector A vector of classical orbital elements
/// @param [in] aThrustAcceleration The thrust acceleration
///
/// @return The maximal change in orbital elements

Vector5d computeOrbitalElementsMaximalChange(const Vector5d& aCOEVector, const double& aThrustAcceleration) const;

/// @brief Compute the Proximity Quotient value
///
/// @param [in] aCOEVector The vector of classical orbital elements
/// @param [in] aThrustAcceleration The thrust acceleration
///
/// @return The Q value

double computeQ(const Vector5d& aCOEVector, const double& aThrustAcceleration) const;

/// @brief Compute the derivative of Q with respect to the orbital elements
///
/// @param [in] aCOEVector The current vector of classical orbital elements
/// @param [in] aThrustAcceleration The thrust acceleration
///
/// @return The derivative of Q with respect to the orbital elements

Vector5d compute_dQ_dOE(const Vector5d& aCOEVector, const double& aThrustAcceleration) const;

/// @brief Compute the thrust direction
///
/// @param [in] aCOEVector The current vector of classical orbital elements
/// @param [in] aThrustAcceleration The thrust acceleration
///
/// @return The thrust direction

Vector3d computeThrustDirection(const Vector6d& aCOEVector, const double& aThrustAcceleration) const;

/// @brief Compute the derivative of the orbital elements with respect to the thrust vectors
///
/// @param [in] aCOEVector A vector of classical orbital elements
/// @param [in] aGravitationalParameter The gravitational parameter
///
/// @return The derivative of the orbital elements with respect to the thrust vectors

static Matrix53d Compute_dOE_dF(const Vector6d& aCOEVector, const Derived& aGravitationalParameter);

/// @brief Convert from the theta-RH frame to the GCRF frame
///
/// @param [in] aPositionCoordinates The position coordinates
/// @param [in] aVelocityCoordinates The velocity coordinates
///
/// @return The coordinates in the GCRF frame

static Matrix3d ThetaRHToGCRF(const Vector3d& aPositionCoordinates, const Vector3d& aVelocityCoordinates);

private:
Parameters parameters_;
const Real mu_;
const double mu_;
const Vector6d targetCOEVector_;
const Derived gravitationalParameter_;
const FiniteDifferenceSolver finiteDifferenceSolver_;
const StateBuilder stateBuilder_;

Real computeQ(const Vector5d& currentCOEVector, const Real& aThrustAcceleration) const;

Vector3d computeThrustDirection(const Vector6d& currentCOEVector, const Real& aThrustAcceleration) const;

Matrix53d computeDOEWithF(const Vector6d& aCOEVector) const;

Vector5d computeOrbitalElementsMaximalChange(const Vector5d& aCOEVector, const Real& aThrustAcceleration) const;

static Matrix3d thetaRHToGCRF(const Vector3d& aPositionCoordinates, const Vector3d& aVelocityCoordinates);
};

} // namespace guidancelaw
Expand Down
Loading

0 comments on commit f477d29

Please sign in to comment.