Skip to content

Commit

Permalink
Convert ReturnFromMoon maneuver to use analytical Jacobian
Browse files Browse the repository at this point in the history 
This doesn't seem to do that much in terms of speedup or stability
really (although I didn't try investigating stability very well).

Investigating the values in the jacobian one of the biggest problems is
clearly integrating back from the terminal endpoints.  The biggest
numbers come from the state transition matrix there.  This is likely
because small target periapsis constraints results in highly eccentric
return orbits.  Taking problems that blow up and using a higher target
periapsis often results in convergence.

There is a note in Ellison & Englander (2019) that might be applicable:

> A more natural propagation strategy would be to utilize time
> eegularization, such a Sundman transformation,18–20 and a
> corresponding modification to the variational equations. This
> is left as future work.

It might also be possible to use some kind of "homotopy" and if the
problem fails, relax the periapsis constraint to some higher
intermediate point that is easier to solve, then use that as an initial
guess.  I'd really like to avoid iterative approaches.

Also I have not investigated how good/bad my initial guessing is, and it
may be possible that in the cases which do not converge that it is very
poor.

I think it may also work to target an Apoapsis now (higher orbit than
the moon around the primary), although I didn't test that a lot.

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
  • Loading branch information
@lamont-granquist
lamont-granquist committed Aug 23, 2023
1 parent 7992f60 commit dc1bf95
Show file tree
Hide file tree
Showing 7 changed files with 273 additions and 47 deletions.
8 changes: 8 additions & 0 deletions MechJeb2/MechJebLib/Maneuvers/ChangeOrbitalElement.cs
View file
Expand Up @@ -194,6 +194,14 @@ public static V3 ChangeApoapsis(double mu, V3 r, V3 v, double apR, bool optguard
return DeltaV(mu, r, v, apR, Type.APOAPSIS, optguard);
}

public static V3 ChangeApsis(double mu, V3 r, V3 v, double valueR, bool optguard = false)
{
if (!valueR.IsFinite())
throw new ArgumentException($"Bad apoapsis in ChangeOrbitalElement = {valueR}");

return valueR <= r.magnitude ? DeltaV(mu, r, v, valueR, Type.PERIAPSIS, optguard) : DeltaV(mu, r, v, valueR, Type.APOAPSIS, optguard);
}

public static V3 ChangeSMA(double mu, V3 r, V3 v, double sma, bool optguard = false)
{
if (!sma.IsFinite())
Expand Down
225 changes: 180 additions & 45 deletions MechJeb2/MechJebLib/Maneuvers/ReturnFromMoon.cs
View file
Expand Up @@ -29,12 +29,13 @@ private struct Args
public Scale MoonToPlanetScale;
}

private static void NLPFunction(double[] x, double[] fi, object obj)
private static void NLPFunction(double[] x, double[] fi, double[,] jac, object o)
//private static void NLPFunction(double[] x, double[] fi, object obj)
{
/*
* unpacking constants
*/
var args = (Args)obj;
var args = (Args)o;

double moonSOI = args.MoonSOI;
double peR = args.PeR;
Expand All @@ -54,54 +55,171 @@ private static void NLPFunction(double[] x, double[] fi, object obj)
double moonDt2 = x[5];
V3 rsoi = new V3(moonSOI, x[6], x[7]).sph2cart;
V3 vsoi = new V3(x[8], x[9], x[10]).sph2cart;
double planetDt1 = x[11];
double planetDt2 = x[12];
var rf = new V3(x[13], x[14], x[15]);
var vf = new V3(x[16], x[17], x[18]);
double moonDt = x[11];
double planetDt1 = x[12];
double planetDt2 = x[13];
var rf = new V3(x[14], x[15], x[16]);
var vf = new V3(x[17], x[18], x[19]);

// forward propagation in the moon SOI
(V3 rburn, V3 vburn) = Shepperd.Solve(1.0, burnTime, r0, v0);
(V3 r1Minus, V3 v1Minus) = Shepperd.Solve(1.0, moonDt1, rburn, vburn + dv);
V3 aburn = -rburn / (rburn.sqrMagnitude * rburn.magnitude);
(V3 r1Minus, V3 v1Minus, M3 v1MinusS00, M3 v1MinusS01, M3 v1MinusS10, M3 v1MinusS11) = Shepperd.Solve2(1.0, moonDt1, rburn, vburn + dv);
V3 a1Minus = -r1Minus / (r1Minus.sqrMagnitude * r1Minus.magnitude);

// reverse propagation in the moon SOI
(V3 r1Plus, V3 v1Plus) = Shepperd.Solve(1.0, -moonDt2, rsoi, vsoi);
(V3 r1Plus, V3 v1Plus, M3 v1PlusS00, M3 v1PlusS01, M3 v1PlusS10, M3 v1PlusS11) = Shepperd.Solve2(1.0, -moonDt2, rsoi, vsoi);
V3 a1Plus = -r1Plus / (r1Plus.sqrMagnitude * r1Plus.magnitude);

// propagation of the moon via the "ephemeris"
double t2 = (moonDt1 + moonDt2 + burnTime) / moonToPlanetScale.TimeScale;
double t2 = moonDt / moonToPlanetScale.TimeScale;
(V3 moonR2, V3 moonV2) = Shepperd.Solve(1.0, t2, moonR0, moonV0);
V3 moonA2 = -moonR2 / (moonR2.sqrMagnitude * moonR2.magnitude);

V3 rsoiPlanet = rsoi / moonToPlanetScale.LengthScale + moonR2;
V3 vsoiPlanet = vsoi / moonToPlanetScale.VelocityScale + moonV2;

// forward propagation in the planet SOI
(V3 r2Minus, V3 v2Minus) = Shepperd.Solve(1.0, planetDt1, rsoiPlanet, vsoiPlanet);
(V3 r2Minus, V3 v2Minus, M3 v2MinusS00, M3 v2MinusS01, M3 v2MinusS10, M3 v2MinusS11) = Shepperd.Solve2(1.0, planetDt1, rsoiPlanet, vsoiPlanet);
V3 a2Minus = -r2Minus / (r2Minus.sqrMagnitude * r2Minus.magnitude);

// reverse propagation in the planet SOI
(V3 r2Plus, V3 v2Plus) = Shepperd.Solve(1.0, -planetDt2, rf, vf);
(V3 r2Plus, V3 v2Plus, M3 v2PlusS00, M3 v2PlusS01, M3 v2PlusS10, M3 v2PlusS11) = Shepperd.Solve2(1.0, -planetDt2, rf, vf);
V3 a2Plus = -r2Plus / (r2Plus.sqrMagnitude * r2Plus.magnitude);

// objective
fi[0] = dv.sqrMagnitude;
Dual obj = new DualV3(dv, new V3(1, 0, 0)).sqrMagnitude;
fi[0] = obj.M;
jac[0, 0] = obj.D;
jac[0, 1] = new DualV3(dv, new V3(0, 1, 0)).sqrMagnitude.D;
jac[0, 2] = new DualV3(dv, new V3(0, 0, 1)).sqrMagnitude.D;

/*
* meet in the moon's SOI
*/

// meet in the moon's SOI
fi[1] = r1Minus.x - r1Plus.x;
fi[2] = r1Minus.y - r1Plus.y;
fi[3] = r1Minus.z - r1Plus.z;
fi[4] = v1Minus.x - v1Plus.x;
fi[5] = v1Minus.y - v1Plus.y;
fi[6] = v1Minus.z - v1Plus.z;
// meet in the planet's SOI
fi[7] = r2Minus.x - r2Plus.x;
fi[8] = r2Minus.y - r2Plus.y;
fi[9] = r2Minus.z - r2Plus.z;
fi[10] = v2Minus.x - v2Plus.x;
fi[11] = v2Minus.y - v2Plus.y;
fi[12] = v2Minus.z - v2Plus.z;
// end at the periapsis
fi[13] = V3.Dot(rf.normalized, vf.normalized);
fi[14] = rf.magnitude * rf.magnitude - peR * peR;
// put some constraints on when the midpoints meet
fi[15] = moonDt1 - moonDt2;
fi[16] = planetDt1 - planetDt2;

v1MinusS01.CopyTo(jac, 1, 0);
v1MinusS11.CopyTo(jac, 4, 0);

(v1MinusS00 * vburn + v1MinusS01 * aburn).CopyTo(jac, 1,3);
(v1MinusS10 * vburn + v1MinusS11 * aburn).CopyTo(jac, 4,3);

v1Minus.CopyTo(jac, 1, 4);
a1Minus.CopyTo(jac, 4, 4);

v1Plus.CopyTo(jac, 1, 5);
a1Plus.CopyTo(jac, 4, 5);

DualV3 rsoi6 = new DualV3(moonSOI, x[6], x[7], 0, 1, 0).sph2cart;
DualV3 rsoi7 = new DualV3(moonSOI, x[6], x[7], 0, 0, 1).sph2cart;

var d2 = new M3(rsoi6.D, rsoi7.D, V3.zero);

DualV3 vsoi8 = new DualV3(x[8], x[9], x[10], 1, 0, 0).sph2cart;
DualV3 vsoi9 = new DualV3(x[8], x[9], x[10], 0, 1, 0).sph2cart;
DualV3 vsoi10 = new DualV3(x[8], x[9], x[10], 0, 0, 1).sph2cart;

var d3 = new M3(vsoi8.D, vsoi9.D, vsoi10.D);

(-v1PlusS00*d2).CopyTo(jac, 1, 6);
(-v1PlusS01*d3).CopyTo(jac, 1, 8);
(-v1PlusS10*d2).CopyTo(jac, 4, 6);
(-v1PlusS11*d3).CopyTo(jac, 4, 8);

/*
* meet in the planet's SOI
*/

fi[7] = r2Minus.x - r2Plus.x;
fi[8] = r2Minus.y - r2Plus.y;
fi[9] = r2Minus.z - r2Plus.z;
fi[10] = v2Minus.x - v2Plus.x;
fi[11] = v2Minus.y - v2Plus.y;
fi[12] = v2Minus.z - v2Plus.z;

double l = moonToPlanetScale.LengthScale;
double v = moonToPlanetScale.VelocityScale;
double t = moonToPlanetScale.TimeScale;

DualV3 rsoiPlanet6 = new DualV3(moonSOI, x[6], x[7], 0, 1, 0).sph2cart/ l + moonR2;
DualV3 rsoiPlanet7 = new DualV3(moonSOI, x[6], x[7], 0, 0, 1).sph2cart/ l + moonR2;

var d0 = new M3(rsoiPlanet6.D, rsoiPlanet7.D, V3.zero);

DualV3 vsoiPlanet8 = new DualV3(x[8], x[9], x[10], 1, 0, 0).sph2cart / v + moonV2;
DualV3 vsoiPlanet9 = new DualV3(x[8], x[9], x[10], 0, 1, 0).sph2cart / v + moonV2;
DualV3 vsoiPlanet10 = new DualV3(x[8], x[9], x[10], 0, 0, 1).sph2cart / v + moonV2;

var d1 = new M3(vsoiPlanet8.D, vsoiPlanet9.D, vsoiPlanet10.D);

(v2MinusS00*d0).CopyTo(jac, 7, 6);
(v2MinusS01*d1).CopyTo(jac, 7, 8);
(v2MinusS10*d0).CopyTo(jac, 10, 6);
(v2MinusS11*d1).CopyTo(jac, 10, 8);

((v2MinusS00 * moonV2 + v2MinusS01 * moonA2) / t).CopyTo(jac, 7,11);
((v2MinusS10 * moonV2 + v2MinusS11 * moonA2) / t).CopyTo(jac, 10,11);

v2Minus.CopyTo(jac, 7, 12);
a2Minus.CopyTo(jac, 10, 12);

v2Plus.CopyTo(jac, 7, 13);
a2Plus.CopyTo(jac, 10, 13);

(-v2PlusS00).CopyTo(jac, 7, 14);
(-v2PlusS01).CopyTo(jac, 7, 17);
(-v2PlusS10).CopyTo(jac, 10, 14);
(-v2PlusS11).CopyTo(jac, 10, 17);

/*
* periapsis condition
*/

Dual p = DualV3.Dot(new DualV3(rf, new V3(1, 0, 0)).normalized, vf.normalized);
fi[13] = p.M;
jac[13, 14] = p.D;
jac[13, 15] = DualV3.Dot(new DualV3(rf, new V3(0, 1, 0)).normalized, vf.normalized).D;
jac[13, 16] = DualV3.Dot(new DualV3(rf, new V3(0, 0, 1)).normalized, vf.normalized).D;
jac[13, 17] = DualV3.Dot(rf.normalized, new DualV3(vf, new V3(1, 0, 0)).normalized).D;
jac[13, 18] = DualV3.Dot(rf.normalized, new DualV3(vf, new V3(0, 1, 0)).normalized).D;
jac[13, 19] = DualV3.Dot(rf.normalized, new DualV3(vf, new V3(0, 0, 1)).normalized).D;

/*
* periapsis constraint
*/

fi[14] = rf.sqrMagnitude - peR * peR;
jac[14, 14] = (new DualV3(rf, new V3(1, 0, 0)).sqrMagnitude - peR * peR).D;
jac[14, 15] = (new DualV3(rf, new V3(0, 1, 0)).sqrMagnitude - peR * peR).D;
jac[14, 16] = (new DualV3(rf, new V3(0, 0, 1)).sqrMagnitude - peR * peR).D;

/*
* time constraints
*/

fi[15] = moonDt1 + moonDt2 + burnTime - moonDt;
jac[15, 3] = 1.0;
jac[15, 4] = 1.0;
jac[15, 5] = 1.0;
jac[15, 11] = -1.0;

/*
* midpoint time constraints
*/

fi[16] = moonDt1 - moonDt2;
jac[16, 4] = 1.0;
jac[16, 5] = -1.0;
fi[17] = planetDt1 - planetDt2;
jac[17, 12] = 1.0;
jac[17, 13] = -1.0;
}

[UsedImplicitly]
Expand All @@ -114,7 +232,7 @@ private static void NLPFunction(double[] x, double[] fi, object obj)
const double EPSX = 1e-4;
const int MAXITS = 1000;

const int NVARIABLES = 19;
const int NVARIABLES = 20;
const int NEQUALITYCONSTRAINTS = 17;
const int NINEQUALITYCONSTRAINTS = 0;

Expand Down Expand Up @@ -144,7 +262,7 @@ private static void NLPFunction(double[] x, double[] fi, object obj)
if (ecc < 1)
{
// approximate vsoi with the amount to kick the moon's orbit down to the periapsis
V3 v1 = ChangeOrbitalElement.ChangePeriapsis(centralMu, moonR0, moonV0, peR);
V3 v1 = ChangeOrbitalElement.ChangeApsis(centralMu, moonR0, moonV0, peR);

// then do the source moon SOI
V3 vneg, vpos, rburn;
Expand All @@ -167,9 +285,8 @@ private static void NLPFunction(double[] x, double[] fi, object obj)
// propagate the moon forward to the estimate of the time of the burn
(V3 moonR2, V3 moonV2) = Shepperd.Solve(centralMu, dt, moonR0, moonV0);

// construct rf + vf by propagating the moon forward to the burn time and changing the PeR to find a
// somewhat feasible rf+vf
V3 moonV3 = moonV2 + ChangeOrbitalElement.ChangePeriapsis(centralMu, moonR2, moonV2, peR);
// construct rf + vf by propagating the moon forward to the burn time and changing the PeR to find a feasible rf+vf
V3 moonV3 = moonV2 + ChangeOrbitalElement.ChangeApsis(centralMu, moonR2, moonV2, peR);
double tt2 = Maths.TimeToNextPeriapsis(centralMu, moonR0, moonV3);
(rf, vf) = Shepperd.Solve(centralMu, tt2, moonR0, moonV3);

Expand All @@ -192,14 +309,15 @@ private static void NLPFunction(double[] x, double[] fi, object obj)
x[8] = v2Sph[0]; // r of SOI velocity
x[9] = v2Sph[1]; // theta of SOI velocity
x[10] = v2Sph[2]; // phi of SOI velocity
x[11] = tt2 / 2; // 1/2 coast time through planet SOI
x[12] = tt2 / 2; // 1/2 coast time thorugh planet SOI
x[13] = rf.x; // final rx
x[14] = rf.y; // final ry
x[15] = rf.z; // final rz
x[16] = vf.x; // final vx
x[17] = vf.y; // final vy
x[18] = vf.z; // final vz
x[11] = tt1; // coast time through moon SOI
x[12] = tt2 / 2; // 1/2 coast time through planet SOI
x[13] = tt2 / 2; // 1/2 coast time thorugh planet SOI
x[14] = rf.x; // final rx
x[15] = rf.y; // final ry
x[16] = rf.z; // final rz
x[17] = vf.x; // final vx
x[18] = vf.y; // final vy
x[19] = vf.z; // final vz

var args = new Args
{
Expand All @@ -210,28 +328,45 @@ private static void NLPFunction(double[] x, double[] fi, object obj)
V0 = v0 / moonScale.VelocityScale,
MoonR0 = moonR0 / planetScale.LengthScale,
MoonV0 = moonV0 / planetScale.VelocityScale,
MoonToPlanetScale = moonToPlanetScale,
MoonToPlanetScale = moonToPlanetScale
};

alglib.minnlccreatef(NVARIABLES, x, DIFFSTEP, out alglib.minnlcstate state);
//alglib.minnlccreatef(NVARIABLES, x, DIFFSTEP, out alglib.minnlcstate state);
alglib.minnlccreate(NVARIABLES, x, out alglib.minnlcstate state);

alglib.minnlcsetbc(state, bndl, bndu);
alglib.minnlcsetstpmax(state, 1e-8);
alglib.minnlcsetalgosqp(state);
alglib.minnlcsetcond(state, EPSX, MAXITS);
alglib.minnlcsetnlc(state, NEQUALITYCONSTRAINTS, NINEQUALITYCONSTRAINTS);

/*
alglib.minnlcoptguardsmoothness(state);
alglib.minnlcoptguardgradient(state, DIFFSTEP);
*/

alglib.minnlcoptimize(state, NLPFunction, null, args);
alglib.minnlcresults(state, out x, out alglib.minnlcreport rep);

/*
alglib.minnlcoptguardresults(state, out alglib.optguardreport ogrep);
if (ogrep.badgradsuspected)
throw new Exception(
$"badgradsuspected: {ogrep.badgradfidx},{ogrep.badgradvidx}\nuser:\n{DoubleMatrixString(ogrep.badgraduser)}\nnumerical:\n{DoubleMatrixString(ogrep.badgradnum)}\nsparsity check:\n{DoubleMatrixSparsityCheck(ogrep.badgraduser, ogrep.badgradnum, 1e-2)}");
if (ogrep.nonc0suspected || ogrep.nonc1suspected)
throw new Exception("alglib optguard caught an error, i should report better on errors now");
*/

if (rep.terminationtype < 0)
throw new Exception(
$"ReturnFromMoon.Maneuver(): SQP solver terminated abnormally: {rep.terminationtype}"
);

NLPFunction(x, fi, args);
fi[0] = 0; // zero out the objective
if (DoubleArrayMagnitude(fi) > 1e-4)
if (rep.nlcerr > 1e-4)
throw new Exception(
$"ReturnFromMoon.Maneuver() no feasible solution found: constraint violation: {DoubleArrayMagnitude(fi)}");
$"ReturnFromMoon.Maneuver() no feasible solution found, constraint violation: {rep.nlcerr}");

return (new V3(x[0], x[1], x[2]) * moonScale.VelocityScale, x[3] * moonScale.TimeScale);
}
Expand Down
3 changes: 2 additions & 1 deletion MechJeb2/MechJebLib/Primitives/DualV3.cs
View file
Expand Up @@ -18,7 +18,7 @@ public class DualV3 : IEquatable<DualV3>, IComparable<DualV3>, IFormattable
public Dual y => new Dual(M.y, D.y);
public Dual z => new Dual(M.z, D.z);

private DualV3(V3 m, V3? dx = null)
public DualV3(V3 m, V3? dx = null)
{
M = m;
D = dx ?? V3.zero;
Expand All @@ -30,6 +30,7 @@ private DualV3(V3 m, V3? dx = null)
public Dual sqrMagnitude => x * x + y * y + z * z;
public Dual magnitude => Dual.Sqrt(x * x + y * y + z * z);
public DualV3 normalized => this / magnitude;
public DualV3 sph2cart => x * new DualV3(Dual.Cos(z) * Dual.Sin(y), Dual.Sin(z) * Dual.Sin(y), Dual.Cos(y));

public static DualV3 operator +(DualV3 a, DualV3 b) => new DualV3(a.x + b.x, a.y + b.y, a.z + b.z);

Expand Down
7 changes: 7 additions & 0 deletions MechJeb2/MechJebLib/Primitives/M3.cs
View file
Expand Up @@ -563,6 +563,13 @@ public double min_magnitude
}
}

public void CopyTo(double[,] other, int x, int y)
{
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
other[x + i, y + j] = this[i, j];
}

// TODO:
// private QuaternionD GetRotation();
// public static M3 LookAt(V3 from, V3 to, V3 up);
Expand Down
9 changes: 9 additions & 0 deletions MechJeb2/MechJebLib/Primitives/V3.cs
View file
Expand Up @@ -351,6 +351,8 @@ public V3 cart2sph

public static V3 operator /(V3 a, double d) => new V3(a.x / d, a.y / d, a.z / d);

public static V3 operator /(double d, V3 a) => new V3(d/a.x, d/a.y, d/a.z);

public static bool operator ==(V3 lhs, V3 rhs)
{
double diff_X = lhs.x - rhs.x;
Expand Down Expand Up @@ -389,5 +391,12 @@ public void CopyTo(IList<double> other, int index = 0)
other[index + 1] = this[1];
other[index + 2] = this[2];
}

public void CopyTo(double[,] other, int i, int j)
{
other[i,j] = this[0];
other[i + 1,j] = this[1];
other[i + 2,j] = this[2];
}
}
}

0 comments on commit dc1bf95

Please sign in to comment.