From 4c3a8f2b425e7d5a23658a7141179fa241fc752a Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Fri, 11 Aug 2023 13:44:36 -0700 Subject: [PATCH] Additional box pinning in the PVG optimizer pin r0, v0 and the burntime of fixed phases Signed-off-by: Lamont Granquist --- MechJeb2/MechJebLib/PVG/InputLayout.cs | 10 +++++++-- MechJeb2/MechJebLib/PVG/Optimizer.cs | 30 ++++++++++++++++++++------ 2 files changed, 32 insertions(+), 8 deletions(-) diff --git a/MechJeb2/MechJebLib/PVG/InputLayout.cs b/MechJeb2/MechJebLib/PVG/InputLayout.cs index 51492181..ab864fdf 100644 --- a/MechJeb2/MechJebLib/PVG/InputLayout.cs +++ b/MechJeb2/MechJebLib/PVG/InputLayout.cs @@ -13,9 +13,15 @@ namespace MechJebLib.PVG { public struct InputLayout { - public const int INPUT_LAYOUT_LEN = 15; - public const int BT_INDEX = 14; + public const int RX_INDEX = 0; + public const int RY_INDEX = 1; + public const int RZ_INDEX = 2; + public const int VX_INDEX = 3; + public const int VY_INDEX = 4; + public const int VZ_INDEX = 5; public const int M_INDEX = 12; + public const int BT_INDEX = 14; + public const int INPUT_LAYOUT_LEN = 15; public V3 R; diff --git a/MechJeb2/MechJebLib/PVG/Optimizer.cs b/MechJeb2/MechJebLib/PVG/Optimizer.cs index 7c3c2aec..60e8bf97 100644 --- a/MechJeb2/MechJebLib/PVG/Optimizer.cs +++ b/MechJeb2/MechJebLib/PVG/Optimizer.cs @@ -233,12 +233,29 @@ private void UnSafeRun() for (int i = 0; i < _phases.Count; i++) { - if (!_phases[i].Coast && !_phases[i].Infinite) + // pin the maximum time of any finite burn phase to below the tau value of the stage + if (!_phases[i].Coast && !_phases[i].Infinite && _phases[i].OptimizeTime) bndu[i * InputLayout.INPUT_LAYOUT_LEN + InputLayout.BT_INDEX] = _phases[i].tau * 0.999; + + // pin the time of any phase which isn't allowed to be optimized + if (!_phases[i].OptimizeTime) + bndu[i * InputLayout.INPUT_LAYOUT_LEN + InputLayout.BT_INDEX] = + bndl[i * InputLayout.INPUT_LAYOUT_LEN + InputLayout.BT_INDEX] = _phases[i].bt; + + // pin the m0 of the stage based on the value computed for the phase + // FIXME: stage and a half or coasts-within-stages would require dropping this. bndl[i * InputLayout.INPUT_LAYOUT_LEN + InputLayout.M_INDEX] = _phases[i].m0; bndu[i * InputLayout.INPUT_LAYOUT_LEN + InputLayout.M_INDEX] = _phases[i].m0; } + // pin r0 and v0 by box equality constraints in the optimizer + bndl[InputLayout.RX_INDEX] = bndu[InputLayout.RX_INDEX] = _problem.R0.x; + bndl[InputLayout.RY_INDEX] = bndu[InputLayout.RY_INDEX] = _problem.R0.y; + bndl[InputLayout.RZ_INDEX] = bndu[InputLayout.RZ_INDEX] = _problem.R0.z; + bndl[InputLayout.VX_INDEX] = bndu[InputLayout.VX_INDEX] = _problem.V0.x; + bndl[InputLayout.VY_INDEX] = bndu[InputLayout.VY_INDEX] = _problem.V0.y; + bndl[InputLayout.VZ_INDEX] = bndu[InputLayout.VZ_INDEX] = _problem.V0.z; + alglib.minlmcreatev(ResidualLayout.RESIDUAL_LAYOUT_LEN * _phases.Count, yGuess, LmDiffStep, out _state); alglib.minlmsetbc(_state, bndl, bndu); alglib.minlmsetcond(_state, LmEpsx, MaxIter); @@ -318,9 +335,10 @@ private void Shooting(Solution? solution = null) { y0.R = _problem.R0; y0.V = _problem.V0; - y0.M = _problem.M0; } + y0.M = phase.m0; + y0.CopyTo(_initial[p]); double bt = phase.OptimizeTime ? y0.Bt : phase.bt; @@ -365,7 +383,6 @@ public Optimizer Bootstrap(V3 pv0, V3 pr0) { y0.R = _problem.R0; y0.V = _problem.V0; - y0.M = phase.m0; y0.PV = pv0; y0.PR = pr0; y0.Bt = phase.bt; @@ -375,9 +392,10 @@ public Optimizer Bootstrap(V3 pv0, V3 pr0) _terminal[p - 1].CopyTo(_initial[p]); y0.CopyFrom(_initial[p]); y0.Bt = phase.bt; - y0.M = phase.m0; } + y0.M = phase.m0; + y0.CopyTo(_initial[p]); double tf = t0 + y0.Bt; @@ -450,7 +468,6 @@ public Optimizer Bootstrap(Solution solution) { y0.R = _problem.R0; y0.V = _problem.V0; - y0.M = _problem.M0; y0.Bt = phase.bt; y0.PV = solution.Pv(_problem.T0); y0.PR = solution.Pr(_problem.T0); @@ -460,9 +477,10 @@ public Optimizer Bootstrap(Solution solution) _terminal[p - 1].CopyTo(_initial[p]); y0.CopyFrom(_initial[p]); y0.Bt = phase.bt; - y0.M = phase.m0; } + y0.M = phase.m0; + y0.CopyTo(_initial[p]); double tf = t0 + y0.Bt;