def calcOrbit(self):
errEAN_L = 0 * pi/180
errArgPe = 0# -3.91 * pi/180
timeErr = 0
# we try to first convert keplerian elements (assuming ArgPe = 0) to carthesian at t1, then back-propagate to tL to get r, then TAN_L then ArgPe. Hopefully....
print("------------------------------------------------------")
#First, fisrt guess at t1 :
self.OrbitPoints[1].r, self.OrbitPoints[1].v = par2ic([self.SMA,self.e,0,0,0,self.OrbitPoints[1].EAN],self.mu_c)
#Then back propagate to tL :
self.OrbitPoints[0].r, self.OrbitPoints[0].v = propagate_lagrangian(self.OrbitPoints[1].r,self.OrbitPoints[1].v,self.OrbitPoints[0].t-self.OrbitPoints[1].t,self.mu_c) #-t0_M should be tL
self.printVect(self.OrbitPoints[0].r,"rLaunch pred")
self.printVect(self.OrbitPoints[0].v,"vLaunch pred")
print("Launch influence free radius : ",np.round(self.OrbitPoints[0].r_n/1000,1)," [km]")
print("Launch radius delta from K orbit : ",np.round(self.OrbitPoints[0].r_n/1000-norm(self.OrbitPoints[0].r)/1000,1)," [km]")
#we need the sign for the TAN, i.e. are we before or after Pe Do this with a small delta T
dt = 1
rLdt, vLdt = propagate_lagrangian(self.OrbitPoints[1].r,self.OrbitPoints[1].v,self.OrbitPoints[0].t-self.OrbitPoints[1].t+dt,self.mu_c) #measure dr from dt.
# if dr is increasing with dt, Pe is past, if decreasing Pe is in front of us
sign = 1
if norm(self.OrbitPoints[0].r)-norm(rLdt) < 0 :
sign = -1
self.OrbitPoints[0].SetAN(self.SMA,self.e,sign,"fromVector")
self.ArgPe = self.calcArgPe(self.OrbitPoints[0].t)+errArgPe #now that we have a proper radius (virtual radius ignoring kerbin influence at tL, we get a proper trueAnom and can get the ArgPe)
#self.ArgPe = (17.9)*pi/180
print("------------------------------------------------------")
print("TAN at Launch : ",self.OrbitPoints[0].TAN*180/pi)
print("EAN at Launch : ",self.OrbitPoints[0].EAN*180/pi)
print("KTAN at Launch : ",self.KerbinTAN(self.OrbitPoints[0].t)*180/pi)
print("Arg Pe : ",self.ArgPe*180/pi)
print("------------------------------------------------------")
self.OrbitPoints[2].r, self.OrbitPoints[2].v = propagate_lagrangian(self.OrbitPoints[1].r,self.OrbitPoints[1].v,self.OrbitPoints[2].t-self.OrbitPoints[1].t,self.mu_c) #-t0_M should be tL
self.printVect(self.OrbitPoints[2].r,"r2 pred from t1")
self.printVect(self.OrbitPoints[2].v,"v2 pred from t1")
self.deltasAtPoint(norm(self.OrbitPoints[2].r),norm(self.OrbitPoints[2].v),self.OrbitPoints[2].r_n,self.OrbitPoints[2].v_n,"r2","v2")
print("------------------------------------------------------")
# This all good and well, but the orbit epoch is set to t0, which is unpractical.
# For lambert solving, it would be much better to have it set to epoch zero i.e. y1d1 00:00:00
# To do that we need to back propagate to -self.t1, then use r to derive the true anomaly
self.AddOrbitPoint("0:0:0:0:0",0,0) #Orbit at epoch Zero
self.OrbitPoints[-1].r, self.OrbitPoints[-1].v = propagate_lagrangian(self.OrbitPoints[1].r,self.OrbitPoints[1].v,-self.OrbitPoints[1].t,self.mu_c) #we now have r,v at epoch Zero
#print(self.rZero," ",self.vZero)
self.OrbitPoints[-1].SetAN(self.SMA,self.e,-1,"fromVector") # we are definitly before Pe
#self.ArgPe = self.calcArgPe(0)+errArgPe
print("TAN at Epoch Zero : ",self.OrbitPoints[-1].TAN*180/pi)
self.OrbitPoints[-1].r, self.OrbitPoints[-1].v = par2ic([self.SMA,self.e,0,0,self.ArgPe,self.OrbitPoints[-1].EAN],self.mu_c)
r0Z, v0Z = propagate_lagrangian(self.OrbitPoints[-1].r,self.OrbitPoints[-1].v,self.OrbitPoints[1].t,self.mu_c)
self.printVect(r0Z,"r1 pred")
self.printVect(v0Z,"v1 pred")
self.deltasAtPoint(norm(r0Z),norm(v0Z),(self.OrbitPoints[1].r_n),(self.OrbitPoints[1].v_n),"r1 fromZero","v1 fromZero")
r1Z, v1Z = propagate_lagrangian(self.OrbitPoints[-1].r,self.OrbitPoints[-1].v,self.OrbitPoints[2].t,self.mu_c)
self.printVect(r1Z,"r2 pred")
self.printVect(v1Z,"v2 pred")
self.deltasAtPoint(norm(r1Z),norm(v1Z),(self.OrbitPoints[2].r_n),(self.OrbitPoints[2].v_n),"r2 fromZero","v2 fromZero")
def fitness(self, z):
# departure and arrival times
t0 = pk.epoch(0)
tf = pk.epoch(z[0])
# departure and arrival eccentric anomolies
M0 = z[1]
Mf = z[2]
# departure costates
l0 = np.asarray(z[3:])
# set Keplerian elements
elem0 = np.hstack([self.elem0[:5], [M0]])
elemf = np.hstack([self.elemf[:5], [Mf]])
# compute Cartesian states
r0, v0 = pk.par2ic(elem0, self.leg.mu)
rf, vf = pk.par2ic(elemf, self.leg.mu)
# departure and arrival states (xf[6] is unused)
x0 = pk.sims_flanagan.sc_state(r0, v0, self.sc.mass)
xf = pk.sims_flanagan.sc_state(rf, vf, self.sc.mass / 10)
# set leg
self.leg.set(t0, x0, l0, tf, xf)
# equality constraints
ceq = self.leg.mismatch_constraints(atol=self.atol, rtol=self.rtol)
# final mass
obj = self.leg.trajectory[-1, -1]
# Transversality conditions
# At start
lambdas0 = np.array(self.leg.trajectory[0, 7:13])
r0norm = np.sqrt(r0[0] * r0[0] + r0[1] * r0[1] + r0[2] * r0[2])
tmp = - pk.MU_SUN / r0norm**3
tangent = np.array([v0[0], v0[1], v0[2], tmp *
r0[0], tmp * r0[1], tmp * r0[2]])
tangent_norm = np.linalg.norm(tangent)
tangent = tangent / tangent_norm
T0 = np.dot(lambdas0, tangent)
# At end
lambdasf = np.array(self.leg.trajectory[-1, 7:13])
rfnorm = np.sqrt(rf[0] * rf[0] + rf[1] * rf[1] + rf[2] * rf[2])
tmp = - pk.MU_SUN / rfnorm**3
tangent = np.array([vf[0], vf[1], vf[2], tmp *
rf[0], tmp * rf[1], tmp * rf[2]])
tangent_norm = np.linalg.norm(tangent)
tangent = tangent / tangent_norm
Tf = np.dot(lambdasf, tangent)
return np.hstack(([1], ceq, [T0, Tf]))
def fitness(self, z):
# departure and arrival times
t0 = pk.epoch(0)
tf = pk.epoch(z[0])
# departure and arrival eccentric anomolies
M0 = z[1]
Mf = z[2]
# departure costates
l0 = np.asarray(z[3:])
# set Keplerian elements
elem0 = np.hstack([self.elem0[:5], [M0]])
elemf = np.hstack([self.elemf[:5], [Mf]])
# compute Cartesian states
r0, v0 = pk.par2ic(elem0, self.leg.mu)
rf, vf = pk.par2ic(elemf, self.leg.mu)
# departure and arrival states (xf[6] is unused)
x0 = pk.sims_flanagan.sc_state(r0, v0, self.sc.mass)
xf = pk.sims_flanagan.sc_state(rf, vf, self.sc.mass / 10)
# set leg
self.leg.set(t0, x0, l0, tf, xf)
# equality constraints
ceq = self.leg.mismatch_constraints(atol=self.atol, rtol=self.rtol)
# final mass
obj = self.leg.trajectory[-1, -1]
# Transversality conditions
# At start
lambdas0 = np.array(self.leg.trajectory[0, 7:13])
r0norm = np.sqrt(r0[0] * r0[0] + r0[1] * r0[1] + r0[2] * r0[2])
tmp = -pk.MU_SUN / r0norm**3
tangent = np.array(
[v0[0], v0[1], v0[2], tmp * r0[0], tmp * r0[1], tmp * r0[2]])
tangent_norm = np.linalg.norm(tangent)
tangent = tangent / tangent_norm
T0 = np.dot(lambdas0, tangent)
# At end
lambdasf = np.array(self.leg.trajectory[-1, 7:13])
rfnorm = np.sqrt(rf[0] * rf[0] + rf[1] * rf[1] + rf[2] * rf[2])
tmp = -pk.MU_SUN / rfnorm**3
tangent = np.array(
[vf[0], vf[1], vf[2], tmp * rf[0], tmp * rf[1], tmp * rf[2]])
tangent_norm = np.linalg.norm(tangent)
tangent = tangent / tangent_norm
Tf = np.dot(lambdasf, tangent)
return np.hstack(([1], ceq, [T0, Tf]))
def calcOrbit(self):
errEAN_L = 0 * pi/180
errArgPe = 0 * pi/180
timeErr = 0
# we try to first convert keplerian elements (assuming ArgPe = 0) to carthesian at t1, then back-propagate to tL to get r, then TAN_L then ArgPe. Hopefully....
print("------------------------------------------------------")
#First, fisrt guess at t1 :
self.r0, self.v0 = par2ic([self.SMA,self.e,0,0,0,self.EAN_t0],self.mu_c)
#Then back propagate to tL :
self.rL, self.vL = propagate_lagrangian(self.r0,self.v0,-self.t0_M,self.mu_c) #-t0_M should be tL
self.printVect(self.rL,"rL pred")
self.printVect(self.vL,"vL pred")
print("rL influence free radius : ",self.rnL)
print("rL radius delta from K orbit : ",self.rnL-norm(self.rL))
#we need the sign for the TAN, i.e. are we before or after Pe Do this with a small delta T
dt = 1
rLdt, vLdt = propagate_lagrangian(self.r0,self.v0,-self.t0_M+dt,self.mu_c) #measure dr from dt.
# if dr is increasing with dt, Pe is past, if decreasing Pe is in front of us
sign = 1
if norm(self.rL)-norm(rLdt) < 0 :
sign = -1
self.TAN_L = self.calcTAN(norm(self.rL),sign)
self.EAN_L = self.calcEAN(self.TAN_L)
self.ArgPe = self.calcArgPe(self.tL)+errArgPe #now that we have a proper radius (virtual radius ignoring kerbin influence at tL, we get a proper trueAnom and can get the ArgPe)
print("------------------------------------------------------")
print("TAN at Launch : ",self.TAN_L*180/pi)
print("EAN at Launch : ",self.EAN_L*180/pi)
print("KTAN at Launch : ",self.KerbinTAN(self.tL)*180/pi)
print("Arg Pe : ",self.ArgPe*180/pi)
print("------------------------------------------------------")
self.r1, self.v1 = propagate_lagrangian(self.r0,self.v0,self.t1-self.t0,self.mu_c) #-t0_M should be tL
self.printVect(self.r1,"r1 pred")
self.printVect(self.v1,"v1 pred")
self.deltasAtPoint(norm(self.r1),norm(self.v1),self.rn1,self.vn1,"r1","v1")
# This all good and well, but the orbit epoch is set to t0, which is unpractical.
# For lambert solving, it would be much better to have it set to epoch zero i.e. y1d1 00:00:00
# To do that we need to back propagate to -self.t1, then use r to derive the true anomaly
self.rZero, self.vZero = propagate_lagrangian(self.r0,self.v0,-self.t0,self.mu_c) #we now have r,v at epoch Zero
print(self.rZero," ",self.vZero)
self.TAN_Zero = self.calcTAN(norm(self.rZero),-1) # we are definitly before Pe
EAN_ZeroErr = 0*pi/180 ### There is a problem with E at epoch
self.EAN_Zero = self.calcEAN(self.TAN_Zero)+EAN_ZeroErr
#self.ArgPe = self.calcArgPe(0)+errArgPe
print("TAN at Epoch Zero : ",self.TAN_Zero*180/pi)
self.rZero, self.vZero = par2ic([self.SMA,self.e,0,0,self.ArgPe,self.EAN_Zero],self.mu_c)
r0Z, v0Z = propagate_lagrangian(self.rZero,self.vZero,self.t0,self.mu_c)
self.printVect(r0Z,"r0 pred")
self.printVect(v0Z,"v0 pred")
self.deltasAtPoint(norm(r0Z),norm(v0Z),(self.rn0),(self.vn0),"r0fromZero","v0fromZero")
r1Z, v1Z = propagate_lagrangian(self.rZero,self.vZero,self.t1,self.mu_c)
self.printVect(r1Z,"r1 pred")
self.printVect(v1Z,"v1 pred")
self.deltasAtPoint(norm(r1Z),norm(v1Z),(self.rn1),(self.vn1),"r1fromZero","v1fromZero")
def calcOrbit(self):
errEAN_L = 0 * pi / 180
errArgPe = 0 * pi / 180
v0_pred = [0, 0, 0]
r0_pred = [0, 0, 0]
v1_pred = [0, 0, 0]
r1_pred = [0, 0, 0]
# we try to first convert keplerian elements to carthesian, then propagate
print("------------------------------------------------------")
self.rL_data, self.vL_data = par2ic([
self.SMA, self.e, 0, 0, self.ArgPe + errArgPe,
self.EAN_L + errEAN_L
], self.mu_c)
self.printVect(self.rL_data, "rL pred")
self.printVect(self.vL_data, "vL pred")
r0_pred, v0_pred = propagate_lagrangian(self.rL_data, self.vL_data,
self.t0_M, self.mu_c)
self.printVect(r0_pred, "r0 pred")
self.printVect(v0_pred, "v0 pred")
self.deltasAtPoint(norm(r0_pred), norm(v0_pred), self.rn0, self.vn0,
"r0", "v0")
r1_pred, v1_pred = propagate_lagrangian(self.rL_data, self.vL_data,
self.t1_M, self.mu_c)
self.printVect(r1_pred, "r1 pred")
self.printVect(v1_pred, "v1 pred")
self.deltasAtPoint(norm(r1_pred), norm(v1_pred), self.rn1, self.vn1,
"r0", "v0")
'''
def fitness(self, z):
# epochs (mjd2000)
t0 = pk.epoch(0)
tf = pk.epoch(z[0])
# final mass
mf = z[1]
# eccentric anomolies
E0 = z[2]
Ef = z[3]
# controls
u = z[4:]
# set Keplerian elements
self.elem0[5] = E0
self.elemf[5] = Ef
# compute Cartesian states
r0, v0 = pk.par2ic(self.elem0, self.mu)
rf, vf = pk.par2ic(self.elemf, self.mu)
# spacecraft states
x0 = pk.sims_flanagan.sc_state(r0, v0, self.sc.mass)
xf = pk.sims_flanagan.sc_state(rf, vf, mf)
# set leg
self.leg.set(t0, x0, u, tf, xf)
# compute equality constraints
ceq = np.asarray(self.leg.mismatch_constraints(), np.float64)
# nondimensionalise equality constraints
ceq[0:3] /= pk.AU
ceq[3:6] /= pk.EARTH_VELOCITY
ceq[6] /= self.sc.mass
# compute inequality constraints
cineq = np.asarray(self.leg.throttles_constraints(), np.float64)
return np.hstack(([-mf], ceq, cineq))
def fitness(self, z):
# epochs (mjd2000)
t0 = pk.epoch(0)
tf = pk.epoch(z[0])
# final mass
mf = z[1]
# eccentric anomolies
E0 = z[2]
Ef = z[3]
# controls
u = z[4:]
# set Keplerian elements
self.elem0[5] = E0
self.elemf[5] = Ef
# compute Cartesian states
r0, v0 = pk.par2ic(self.elem0, self.mu)
rf, vf = pk.par2ic(self.elemf, self.mu)
# spacecraft states
x0 = pk.sims_flanagan.sc_state(r0, v0, self.sc.mass)
xf = pk.sims_flanagan.sc_state(rf, vf, mf)
# set leg
self.leg.set(t0, x0, u, tf, xf)
# compute equality constraints
ceq = np.asarray(self.leg.mismatch_constraints(), np.float64)
# nondimensionalise equality constraints
ceq[0:3] /= pk.AU
ceq[3:6] /= pk.EARTH_VELOCITY
ceq[6] /= self.sc.mass
# compute inequality constraints
cineq = np.asarray(self.leg.throttles_constraints(), np.float64)
return np.hstack(([-mf], ceq, cineq))
def fitness(self, z):
# times
t0 = pk.epoch(0)
tf = pk.epoch(z[0])
# final eccentric anomaly
Mf = z[1]
# intial costates
l0 = np.asarray(z[2:])
# set arrival Keplerian elements
self.elemf[5] = Mf
# departure state
x0 = pk.sims_flanagan.sc_state(self.x0[0:3], self.x0[3:6], self.x0[6])
# compute Cartesian arrival state
rf, vf = pk.par2ic(self.elemf, self.leg.mu)
xf = pk.sims_flanagan.sc_state(rf, vf, self.sc.mass / 10)
# set leg
self.leg.set(t0, x0, l0, tf, xf)
# equality constraints
ceq = self.leg.mismatch_constraints(atol=self.atol, rtol=self.rtol)
# final mass
# mf = self.leg.trajectory[-1, 6]
# Transversality condition at the end
lambdasf = np.array(self.leg.trajectory[-1, 7:13])
rfnorm = np.sqrt(rf[0] * rf[0] + rf[1] * rf[1] + rf[2] * rf[2])
tmp = - pk.MU_SUN / rfnorm**3
tangent = np.array([vf[0], vf[1], vf[2], tmp *
rf[0], tmp * rf[1], tmp * rf[2]])
tangent_norm = np.linalg.norm(tangent)
tangent = tangent / tangent_norm
Tf = np.dot(lambdasf, tangent)
return np.hstack(([1], ceq, [Tf]))
def fitness(self, z):
# times
t0 = pk.epoch(0)
tf = pk.epoch(z[0])
# final eccentric anomaly
Mf = z[1]
# initial costates
l0 = np.asarray(z[2:])
# set arrival Keplerian elements
self.elemf[5] = Mf
# departure state
x0 = pk.sims_flanagan.sc_state(self.x0[0:3], self.x0[3:6], self.x0[6])
# compute Cartesian arrival state
rf, vf = pk.par2ic(self.elemf, self.leg.mu)
xf = pk.sims_flanagan.sc_state(rf, vf, self.sc.mass / 10)
# set leg
self.leg.set(t0, x0, l0, tf, xf)
# equality constraints
ceq = self.leg.mismatch_constraints(atol=self.atol, rtol=self.rtol)
# final mass
# mf = self.leg.trajectory[-1, 6]
# Transversality condition at the end
lambdasf = np.array(self.leg.trajectory[-1, 7:13])
rfnorm = np.sqrt(rf[0] * rf[0] + rf[1] * rf[1] + rf[2] * rf[2])
tmp = -pk.MU_SUN / rfnorm**3
tangent = np.array(
[vf[0], vf[1], vf[2], tmp * rf[0], tmp * rf[1], tmp * rf[2]])
tangent_norm = np.linalg.norm(tangent)
tangent = tangent / tangent_norm
Tf = np.dot(lambdasf, tangent)
return np.hstack(([1], ceq, [Tf]))
