Skip to content

Commit

Permalink
wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@darioizzo
darioizzo committed Dec 7, 2023
1 parent 0cba931 commit 09c7ef0
Show file tree
Hide file tree
Showing 5 changed files with 67 additions and 70 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,7 @@ set(kep3_SRC_FILES
"${CMAKE_CURRENT_SOURCE_DIR}/src/epoch.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/src/planet.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/src/lambert_problem.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/src/linalg.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/src/udpla/keplerian.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/src/udpla/jpl_lp.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/src/udpla/vsop2013.cpp"
Expand Down
3 changes: 3 additions & 0 deletions include/kep3/leg/sims_flanagan.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,9 @@ class kep3_DLL_PUBLIC sims_flanagan
[[nodiscard]] std::array<double, 7> compute_mismatch_constraints() const;
[[nodiscard]] std::vector<double> compute_throttle_constraints() const;

// Compute gradients (w.r.t. rvm state and w.r.t. throttles, tof)
[[nodiscard]] std::pair<std::array<double, 49>, std::vector<double>> compute_mc_grad() const;

private:
friend class boost::serialization::access;
template <class Archive>
Expand Down
14 changes: 7 additions & 7 deletions pykep/plot/_sf_leg.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,10 +47,10 @@ def add_sf_leg(
:class:`mpl_toolkits.mplot3d.axes3d.Axes3D`: The modified Axes object with the Sims-Flanagan leg added.
"""
# We extract the number of segments from the leg.
n_seg = int(len(sf.throttles) / 3)
dt = sf.tof / n_seg
n_seg_fwd = int(n_seg * sf.cut)
n_seg_bck = n_seg - n_seg_fwd
nseg = int(len(sf.throttles) / 3)
dt = sf.tof / nseg
nseg_fwd = int(nseg * sf.cut)
nseg_bck = nseg - nseg_fwd

# We start the forward pass of the Sims-Flanagan model------------------------------------------------------------------------
pos_fwd = []
Expand All @@ -61,7 +61,7 @@ def add_sf_leg(
# Append to plotting data
pos_fwd.append(rv[0])

for i in range(n_seg_fwd):
for i in range(nseg_fwd):
# compute the dv (first non dimensional)
dv_vec = sf.throttles[3 * i : 3 * i + 3]
throttles_fwd.append(dv_vec)
Expand Down Expand Up @@ -116,9 +116,9 @@ def add_sf_leg(
# Append to plotting data
pos_bck.append(rv[0])

for i in range(n_seg_bck):
for i in range(nseg_bck):
# compute the dv (first non dimensional)
dv_vec = sf.throttles[n_seg_fwd * 3 + 3 * i : n_seg_fwd * 3 + 3 * i + 3]
dv_vec = sf.throttles[nseg_fwd * 3 + 3 * i : nseg_fwd * 3 + 3 * i + 3]
throttles_bck.append(dv_vec)
dv = _np.linalg.norm(dv_vec)
# plot it in a color that is proportional to the strength
Expand Down
58 changes: 14 additions & 44 deletions src/core_astro/stm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,54 +22,24 @@
#include <kep3/core_astro/kepler_equations.hpp>
#include <kep3/core_astro/propagate_lagrangian.hpp>
#include <kep3/core_astro/stm.hpp>

using xt::linalg::inv;
using mat31 = xt::xtensor_fixed<double, xt::xshape<3, 1>>;
using mat13 = xt::xtensor_fixed<double, xt::xshape<1, 3>>;
using mat33 = xt::xtensor_fixed<double, xt::xshape<3, 3>>;
using mat36 = xt::xtensor_fixed<double, xt::xshape<3, 6>>;
using mat66 = xt::xtensor_fixed<double, xt::xshape<6, 6>>;
using mat32 = xt::xtensor_fixed<double, xt::xshape<3, 2>>;
using mat62 = xt::xtensor_fixed<double, xt::xshape<6, 2>>;
using mat61 = xt::xtensor_fixed<double, xt::xshape<6, 1>>;
using mat16 = xt::xtensor_fixed<double, xt::xshape<1, 6>>;
using mat63 = xt::xtensor_fixed<double, xt::xshape<6, 3>>;
#include <kep3/linalg.hpp>

namespace kep3
{

// Linear algebra helpers to speed up xtensor when, small, fixed size matrices and vectors are involved
// TODO(whoever): move somewhere else
// ---------------------------------------------------------------------------------------
template <std::size_t m, std::size_t k, std::size_t n>
xt::xtensor_fixed<double, xt::xshape<m, n>> _dot(const xt::xtensor_fixed<double, xt::xshape<m, k>> &A,
const xt::xtensor_fixed<double, xt::xshape<k, n>> &B)
{
xt::xtensor_fixed<double, xt::xshape<m, n>> C{};
for (decltype(m) i = 0u; i < m; ++i) {
for (decltype(n) j = 0u; j < n; ++j) {
C(i, j) = 0;
for (decltype(k) l = 0u; l < k; ++l) {
{
C(i, j) += A(i, l) * B(l, j);
}
}
}
}
return C;
}

mat33 _skew(const mat31 &v)
{
return {{0., -v(2, 0), v(1, 0)}, {v(2, 0), 0., -v(0, 0)}, {-v(1, 0), v(0, 0), 0.}};
}

mat31 _cross(const mat31 &v1, const mat31 &v2)
{
return {{v1(1, 0) * v2(2, 0) - v2(1, 0) * v1(2, 0), v2(0, 0) * v1(2, 0) - v1(0, 0) * v2(2, 0),
v1(0, 0) * v2(1, 0) - v2(0, 0) * v1(1, 0)}};
}
// ---------------------------------------------------------------------------------------
using xt::linalg::inv;
using kep3::linalg::mat36;
using kep3::linalg::mat16;
using kep3::linalg::mat31;
using kep3::linalg::mat66;
using kep3::linalg::mat63;
using kep3::linalg::mat32;
using kep3::linalg::mat62;
using kep3::linalg::mat61;
using kep3::linalg::mat13;
using kep3::linalg::_dot;
using kep3::linalg::_cross;
using kep3::linalg::_skew;

// Here we take the lagrangian coefficient expressions for rf and vf as function of r0 and v0, and manually,
// differentiate it to obtain the state transition matrix.
Expand Down
61 changes: 42 additions & 19 deletions src/leg/sims_flanagan.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,19 @@
#include <fmt/core.h>
#include <fmt/ranges.h>

#include <xtensor/xadapt.hpp>

#include <kep3/core_astro/constants.hpp>
#include <kep3/core_astro/propagate_lagrangian.hpp>
#include <kep3/epoch.hpp>
#include <kep3/leg/sims_flanagan.hpp>
#include <kep3/linalg.hpp>

namespace kep3::leg
{

//using kep3::linalg::_dot;

void _check_tof(double tof)
{
if (tof < 0.) {
Expand Down Expand Up @@ -207,9 +213,9 @@ double sims_flanagan::get_cut() const
std::array<double, 7> sims_flanagan::compute_mismatch_constraints() const
{
// We start defining the number of forward and backward segments.
auto n_seg = m_throttles.size() / 3u;
auto n_seg_fwd = static_cast<unsigned>(static_cast<double>(n_seg) * m_cut);
auto n_seg_bck = n_seg - n_seg_fwd;
auto nseg = m_throttles.size() / 3u;
auto nseg_fwd = static_cast<unsigned>(static_cast<double>(nseg) * m_cut);
auto nseg_bck = nseg - nseg_fwd;

// We introduce some convenience variables
double max_thrust = get_max_thrust();
Expand All @@ -221,14 +227,14 @@ std::array<double, 7> sims_flanagan::compute_mismatch_constraints() const
// Initial state
std::array<std::array<double, 3>, 2> rv_fwd(get_rvs());
double mass_fwd = get_mf();
double dt = m_tof / static_cast<double>(n_seg);
double dt = m_tof / static_cast<double>(nseg);
// We propagate for a first dt/2 (only if there is at least one forward segment)
if (n_seg_fwd > 0) {
if (nseg_fwd > 0) {
rv_fwd = propagate_lagrangian(rv_fwd, dt / 2, mu, false).first;
}
// We now loop through the forward segments and 1) add a dv + 2) propagate for dt (except on the last segment, where
// we propagate for dt/2).
for (decltype(m_throttles.size()) i = 0u; i < n_seg_fwd; ++i) {
for (decltype(m_throttles.size()) i = 0u; i < nseg_fwd; ++i) {
// We compute the the dv
dv[0] = max_thrust / mass_fwd * dt * m_throttles[3 * i];
dv[1] = max_thrust / mass_fwd * dt * m_throttles[3 * i + 1];
Expand All @@ -241,7 +247,7 @@ std::array<double, 7> sims_flanagan::compute_mismatch_constraints() const
double norm_dv = std::sqrt(dv[0] * dv[0] + dv[1] * dv[1] + dv[2] * dv[2]);
mass_fwd *= std::exp(-norm_dv / isp / kep3::G0);
// Perform the propagation
double prop_duration = (i == n_seg_fwd - 1) ? dt / 2 : dt;
double prop_duration = (i == nseg_fwd - 1) ? dt / 2 : dt;
rv_fwd = propagate_lagrangian(rv_fwd, prop_duration, mu, false).first;
}

Expand All @@ -250,12 +256,12 @@ std::array<double, 7> sims_flanagan::compute_mismatch_constraints() const
std::array<std::array<double, 3>, 2> rv_bck(get_rvf());
double mass_bck = get_mf();
// We propagate for a first dt/2 (only if there is at least one backward segment)
if (n_seg_bck > 0) {
if (nseg_bck > 0) {
rv_bck = propagate_lagrangian(rv_bck, -dt / 2, mu, false).first;
}
// We now loop through the backward segments and 1) add a dv + 2) propagate for -dt (except on the last segment,
// where we propagate for -dt/2).
for (decltype(m_throttles.size()) i = 0u; i < n_seg_bck; ++i) {
for (decltype(m_throttles.size()) i = 0u; i < nseg_bck; ++i) {
// We compute the the dv
dv[0] = max_thrust / mass_bck * dt * m_throttles[m_throttles.size() - 1 - 3 * i - 2];
dv[1] = max_thrust / mass_bck * dt * m_throttles[m_throttles.size() - 1 - 3 * i - 1];
Expand All @@ -268,7 +274,7 @@ std::array<double, 7> sims_flanagan::compute_mismatch_constraints() const
double norm_dv = std::sqrt(dv[0] * dv[0] + dv[1] * dv[1] + dv[2] * dv[2]);
mass_bck *= std::exp(norm_dv / isp / kep3::G0);
// Perform the propagation
double prop_duration = (i == n_seg_bck - 1) ? -dt / 2 : -dt;
double prop_duration = (i == nseg_bck - 1) ? -dt / 2 : -dt;
rv_bck = propagate_lagrangian(rv_bck, prop_duration, mu, false).first;
}
return {rv_fwd[0][0] - rv_bck[0][0], rv_fwd[0][1] - rv_bck[0][1], rv_fwd[0][2] - rv_bck[0][2],
Expand All @@ -278,23 +284,40 @@ std::array<double, 7> sims_flanagan::compute_mismatch_constraints() const

std::vector<double> sims_flanagan::compute_throttle_constraints() const
{
auto n_seg = m_throttles.size() / 3u;
std::vector<double> retval(n_seg);
for (decltype(m_throttles.size()) i = 0u; i < n_seg; ++i) {
auto nseg = m_throttles.size() / 3u;

std::vector<double> retval(nseg);
for (decltype(m_throttles.size()) i = 0u; i < nseg; ++i) {
retval[i] = m_throttles[3 * i] * m_throttles[3 * i] + m_throttles[3 * i + 1] * m_throttles[3 * i + 1]
+ m_throttles[3 * i + 2] * m_throttles[3 * i + 2] - 1.;
}
return retval;
}

std::pair<std::array<double, 49>, std::vector<double>> sims_flanagan::compute_mc_grad() const
{
// Preliminaries
auto nseg = m_throttles.size() / 3u;
//auto nseg_fwd = static_cast<unsigned>(static_cast<double>(nseg) * m_cut);
//auto nseg_bck = nseg - nseg_fwd;
//auto c = m_max_thrust * m_tof / static_cast<double>(nseg); // T*tof/nseg
//auto a = 1. / m_isp / kep3::G0; // 1/veff
//auto dt = m_tof / static_cast<double>(nseg); // dt

// Allocate the return values
std::array<double, 49> grad_rvm{}; // The mismatch constraints gradient w.r.t. extended state r,v,m
std::vector<double> grad(nseg * 3 + 1, 0.); // The mismatch constraints gradient w.r.t. throttles and tof
return {grad_rvm, grad};
}

std::ostream &operator<<(std::ostream &s, const sims_flanagan &sf)
{
auto n_seg = sf.get_throttles().size() / 3u;
auto n_seg_fwd = static_cast<unsigned>(static_cast<double>(n_seg) * sf.get_cut());
auto n_seg_bck = n_seg - n_seg_fwd;
s << fmt::format("Number of segments: {}\n", n_seg);
s << fmt::format("Number of fwd segments: {}\n", n_seg_fwd);
s << fmt::format("Number of bck segments: {}\n", n_seg_bck);
auto nseg = sf.get_throttles().size() / 3u;
auto nseg_fwd = static_cast<unsigned>(static_cast<double>(nseg) * sf.get_cut());
auto nseg_bck = nseg - nseg_fwd;
s << fmt::format("Number of segments: {}\n", nseg);
s << fmt::format("Number of fwd segments: {}\n", nseg_fwd);
s << fmt::format("Number of bck segments: {}\n", nseg_bck);
s << fmt::format("Maximum thrust: {}\n", sf.get_max_thrust());
s << fmt::format("Central body gravitational parameter: {}\n", sf.get_mu());
s << fmt::format("Specific impulse: {}\n\n", sf.get_isp());
Expand Down

0 comments on commit 09c7ef0

Please sign in to comment.