def pterminalcoastU(ic, flow, target, step=1e-2, damping=10, tol=1e-2, iLimit=25, start=0, end=1, param=0, TU=1):
"""
- editing to make it (U)niversal..... 2014_05_16
- could hand function a list of burns with index numbers
related to flow
"""
DEBUG0 = False
DEBUG1 = True
DEBUG2 = False
if DEBUG1:
print ("")
if DEBUG1:
print ("----------------------------------------")
if DEBUG1:
print (" TESTING................. ")
if DEBUG1:
print ("----------------------------------------")
if DEBUG1:
print ("")
# import statements
from flow import P2B
from auxiliary import lambert
from numpy import array, zeros, eye, concatenate
from shootingmodule import firstorder as shoot
# initialize
dt = flow["x"][-1] - flow["x"][0]
dx = step
i = 1
# set initial step direction based on slope
if start == 1 and end == 0:
if res < 0:
dir = -1
if res > 0:
dir = 1
solver = "rk4p"
tstep = 1e-3
mu = ic1[4]
if DEBUG0:
print ("mu is: %5.2f \n" % mu)
if DEBUG0:
print ic1
if DEBUG0:
print ic2
if DEBUG0:
print target
# use absolute value of residual
res = abs(res)
# while-loop unitl tolerance is met
""" modify while-loop statement to run first time through
(i.e. i = 0) like in shootingmodule::firstorder ? ? ?
or maybe not necessary if p, flow is provided.....
"""
while abs(res) > tol:
if DEBUG0:
print ("\n Debug::Iteration is: %i \n" % i)
# save old
if i == 2:
temp = res
if i > 2:
res0 = temp
temp = res
# flip the direction of step if necessary
if i > 120:
if abs(res) > abs(res0):
dir = -1 * dir
# calculate damping needed
elif abs(res) < abs(res0):
slope = (res - res0) / step
dx = -res / slope
"""
if res > 0: sgn = -1; else: sgn = 1
dx = sgn*(res/slope)
if abs(dx/step) < damping: step = step/2
"""
if abs(dx / step) < damping:
step = dx / 2
# ---------------
# flows
# ---------------
# update target #1 and #2 start conditions
# based on updated param
"""
if param > 0:
tspan = [0, param]
elif param < 0:
tspan = [0, -param]
"""
tspan = [0, dir * step]
""" DEBUG """
if DEBUG2:
print ("(dir*step) is: %f \n" % (dir * step))
if DEBUG0:
print ("\n Debug::flow1 \n")
# target #1 flow and ic update
""" could get rid of these if(len()) lines by moving outside while()
and using trim_output = False during while-loop
"""
if len(ic1) == 4:
ic1 = concatenate((ic1.reshape(4, 1), array([[mu]])))
""" choose a time step that is smaller than tspan!!!!"""
if abs(dir * step) < 1e-3:
ministep = abs(tspan[1] - tspan[0]) / 10
else:
ministep = 1e-4
if DEBUG2:
print ("delta-time is %e " % tspan[1])
if DEBUG2:
print ("ministep is: %e " % ministep)
flow1 = P2B(ic1, tspan, tstep=ministep, trim_output=False)
""" DEBUG """
if DEBUG2:
print ("flow(t)")
if DEBUG2:
print flow1["x"][-1] - flow1["x"][0]
if DEBUG2:
print ("flow1(0)....")
if DEBUG2:
print flow1["y"][0][0:4]
if DEBUG2:
print ("flow1(-1)....")
if DEBUG2:
print flow1["y"][-1][0:4]
ic1 = array(flow1["y"][-1])
""" DEBUG """
if DEBUG2:
print ("ic1....")
if DEBUG2:
print ic1[0:4]
if DEBUG0:
print ("\n Debug::flow2 \n")
# target #2 flow and ic update
""" could get rid of these if(len()) lines by moving outside while()
and using trim_output = False during while-loop
"""
if len(ic2) == 4:
ic2 = concatenate((ic2.reshape(4, 1), array([[mu]])))
flow2 = P2B(ic2, tspan, tstep=ministep, trim_output=False)
""" DEBUG """
if DEBUG2:
print ("flow2(-1)....")
if DEBUG2:
print flow2["y"][-1][0:4]
ic2 = array(flow2["y"][-1])
""" DEBUG """
if DEBUG2:
print ("ic2....")
if DEBUG2:
print ic2[0:4]
# solve for new lambert arc
# dt = dt + param
# lamb = prussing_conway(ic1, target, dt, mu, iLimit = 1500, tol = 1E-6);
""" need to update param here because target #1 and target #2
may not have fully stepped the delta-time"""
# update param
param = param + (flow1["x"][-1] - flow1["x"][0])
# skip lambert arc and just applying shooting method
ic3 = zeros(21)
ic3[0:2] = ic1[0:2]
ic3[2:4] = flow["y"][0][2:4]
ic3[4] = mu
# tack identity matrix on end for STM
ic3[5:] = eye(4).reshape(16)
tspan = [0.0, dt - param]
# apply shooting method
if DEBUG0:
print ("\n Debug::ShootingMethod \n")
flow = shoot(
ic3,
target[0:2],
tspan,
eom="P2BP_varEqns",
tstep=1e-2,
trim_output=False,
print_status=False,
damping=1 / 3.0,
)
if DEBUG2:
print ("flow(t)")
if DEBUG2:
print flow["x"][-1] - flow["x"][0]
# if DEBUG0: print ('\n Debug::flow3 \n')
# # target #3 flow
# flow = P2B(solver, tspan, tstep, ic3, TU, \
# eom = 'P2BP_varEqns')
if DEBUG0:
print ("\n Debug::DeltaVs \n")
# -----------------------
# Delta Vs
# -----------------------
dv1 = array([flow["y"][0][2], flow["y"][0][3]]) - ic1[2:4]
dv2 = -array([flow["y"][-1][2], flow["y"][-1][3]]) + target[2:4]
# initial primer vector condition
# dv1 = lamb['v1'] - array([ic1[2], ic1[3]])
# update velocity condition at target
# and calculate final primer vector condition
# if DEBUG0: print ('\n Debug::flow2 Update \n')
# if len(ic2) == 4: ic2 = concatenate((ic2.reshape(4,1), array([[mu]])))
# flow2 = P2B(solver, tspan, tstep, ic2, TU, \
# eom = 'P2BP')
# dv2 = -lamb['v2'] + array([flow2['y'][-1][2], flow2['y'][-1][3]])
if DEBUG0:
print ("\n Debug::Primer History \n")
# primer vector history
p = phistory(dv1, dv2, flow)
if DEBUG0:
print ("\n Debug::Primer Slope (Residual) \n")
# calculate residual
# (i.e. primer vector slope at terminal condition)
res = pslope(flow, p, start, end)
# res = abs(res)
if DEBUG1:
print (
"Iter: %4i \t Res: %9.5f \t Param: %9.6f \t"
"Dir: %2i \t Step: %.6f \t dx: %.6f" % (i, res, param, dir, step, dx)
)
# if limit reached -> break
if i >= iLimit:
break
# update i
i += 1
if DEBUG2:
print ("PVM::2: A BLOCK PRINT ------------ :)")
print (" ~~~~ USING IC ~~~~~")
print ("ic1....")
print ic1[0:4]
print ("ic2....")
print ic2[0:4]
print ("ic3....")
print ic3[0:4]
print (" ~~~~ USING flow ~~~~~")
print ("flow1(-1)....")
print flow1["y"][-1][0:4]
print ("flow2(-1)....")
print flow2["y"][-1][0:4]
print ("flow3(0)....")
print flow["y"][0][0:4]
""" update ic3 for output !!!"""
ic3 = array(flow["y"][0][0:5])
# initialze output dictionary
""" do the following if we request trim_output = True,
otherwise should remove"""
if len(ic1) == 4:
ic1 = concatenate((ic1.reshape(4, 1), array([[mu]])))
if len(ic2) == 4:
ic2 = concatenate((ic2.reshape(4, 1), array([[mu]])))
if i == 1:
ic3 = 0
out = {
"param": param,
"tol": tol,
"res": res,
"iter": i - 1,
"start": start,
"end": end,
"ic1": ic1,
"ic2": ic2,
"ic3": ic3,
"dt": dt,
"x": flow["x"],
"y": flow["y"],
"p": p,
}
if DEBUG1:
print ("")
if DEBUG1:
print ("----------------------------------------")
if DEBUG1:
print (" FINISHED ------ FIN ")
if DEBUG1:
print ("----------------------------------------")
if DEBUG1:
print ("")
return out
def transfer(self, target_position, target_velocity, flight_time = None, \
r_history = None, v_history = None, t_history = None, \
Minimize_Energy = False, SaveFlow = False):
# giving lists to r_history, v_history and t_history arguments will
#+ populate them with the r, v state and time histories
# given export_flow will store the entire flow in export_flow
# Minimize_Energy = True will override 'flight_time' with the
# minimum energy time of flight given from lambert
from utilities import rv2ic
from shootingmodule import firstorder as shoot
# if no flight_time is given,
#+ hopefully the user has switched on Minimize_Energy
if flight_time == None: flight_time = 0.0
# create the initial conditions vector
# select an appropriate eom model that includes the
#+ variational equations
if len(target_position) == 2:
ic = rv2ic(self.r[0:2], self.v[0:2], self.Mu, STM = True)
temp_eom = 'P2BP_varEqns'
elif len(target_position) == 3:
ic = rv2ic(self.r, self.v, self.Mu, STM = True)
temp_eom = 'S2BP_varEqns'
# Override 'flight_time' if Minimize_Energy = True
if Minimize_Energy:
from utilities import create_one_list
from lambert import prussing_conway
from numpy import array
ic0 = create_one_list([self.r, self.v], 0, 1)
icf = create_one_list([target_position, target_velocity], 0, 1)
# find the minimum time for a lambert solution
lambert_solution = prussing_conway(ic0, icf, self.Mu, \
FindMinEnergy = True, \
NonDimUnits = False, \
ScaleOutput = False)
time_for_min_energy = lambert_solution['tm']
# use the minimum time as a flight time to compute
#+ the lambert solution
lambert_solution = prussing_conway(ic0, icf, self.Mu, \
TransferTime = time_for_min_energy, \
NonDimUnits = False, \
ScaleOutput = False)
# calculate the delta-v
dv1_guess = list(lambert_solution['v1'] - array(ic0[2:4]))
dv1_guess.append(0.0)
flight_time = lambert_solution['time']
else:
dv1_guess = None
# apply the first order shooting method
trajectory = shoot(ic, target_position, [0, flight_time], temp_eom, \
tol = 1E-2, iLimit = 60, \
damping = 0.3, guess = dv1_guess, \
print_status = True)
# store the delta-v impulsive maneuvers needed
#+ for the given transfer
if len(target_position) == 2:
self.dv1 = trajectory['y'][0][2:4] - self.v
self.dv2 = target_velocity - trajectory['y'][-1][2:4]
elif len(target_position) == 3:
self.dv1 = trajectory['y'][0][3:6] - self.v
self.dv2 = target_velocity - trajectory['y'][-1][3:6]
# save trajectory states if desired
from utilities import extract_elements
n = len(target_position)
if r_history != None:
extract_elements(trajectory['y'], 0, n - 1, r_history)
else:
extract_elements(trajectory['y'], 0, n - 1, self.r_history)
if v_history != None:
extract_elements(trajectory['y'], n, 2*n - 1, v_history)
else:
extract_elements(trajectory['y'], n, 2*n - 1, self.v_history)
# save the time history of the trajectory
if t_history != None:
t_history = trajectory['x']
else:
self.t_history = trajectory['x']
# if the user wants the entire flow history,
#+ give it to them!
if SaveFlow:
self.theflow = trajectory
def opt_pterminal(x0, ic1, ic2, dt):
"""
1.) flow #1, #2 based on x0
2.) run prussing_conway
3.) run shooting method w/ variational eqns
4.) generate delta-vs
5.) generate primer history
6.) extract slopes
7.) compute cost function
"""
"""
FUTURE WORK
1.) add in intelligent step sizing for flow3 = shoot()
"""
# import statements
from flow import P2B as flow
from lambert import prussing_conway
from shootingmodule import firstorder as shoot
from numpy.linalg import norm
from numpy import array, zeros, eye
print ("Starting an iteration of opt_pterminal!")
print ("x0 is: %e, %e " % (x0[0], x0[1]))
# function parameters
""" FW 1.) """
step = 1e-2
tol = 1e-3
# extract mu
mu = ic1[4]
# flow #1, #2 based on x0[0]
tspan = [0, x0[0]]
ministep = abs(x0[0] / 2e2)
print ("Midpoint-A of an iteration of opt_pterminal!")
print ("tspan is: %e, %e " % (tspan[0], tspan[1]))
print ("ministep is: %e " % ministep)
flow1 = flow(ic1, tspan, ministep, trim_output=False, print_status=True, solver="rk4p")
print ("Midpoint-B of an iteration of opt_pterminal!")
flow2 = flow(ic2, tspan, ministep, trim_output=False)
print ("Midpoint-C of an iteration of opt_pterminal!")
# update ic1 and ic2
ic1 = array(flow1["y"][-1])
ic2 = array(flow2["y"][-1])
# update of flow time based on x0
tspan = [0, dt - x0[0] + x0[1]]
# flow #2 based on x0[1]
flow2 = flow(ic2, tspan, tstep=step, trim_output=False)
# target position
target = array(flow2["y"][-1][0:4])
# solve for lambert arc
print ("Midpoint-0 of an iteration of opt_pterminal!")
arc = prussing_conway(ic1, target, mu, TransferTime=abs(tspan[1]))
# setup initial conditions for flow of spacecraft with
# variational equations
ic = zeros(21)
ic[0:2] = ic1[0:2]
ic[2:4] = arc["v1"]
ic[4] = mu
ic[5:] = eye(4).reshape(16)
# flow spacecraft using lambert arc
print ("Midpoint-1 of an iteration of opt_pterminal!")
flow3 = flow(ic, tspan, tstep=step, eom="P2BP_varEqns", trim_output=False)
print ("Midpoint-2 of an iteration of opt_pterminal!")
# position error
poserror = flow3["y"][-1][0:2] - flow2["y"][-1][0:2]
# apply shooting method to reduce error
if norm(poserror) > tol:
flow3 = shoot(
ic, target[0:2], tspan, eom="P2BP_varEqns", tstep=step, damping=1 / 3.0, tol=tol, trim_output=False
)
poserror = flow3["y"][-1][0:2] - flow2["y"][-1][0:2]
print ("Midpoint-3 of an iteration of opt_pterminal!")
# delta-vs
dv1 = array([flow3["y"][0][2], flow3["y"][0][3]]) - array([ic1[2], ic1[3]])
dv2 = -array([flow3["y"][-1][2], flow3["y"][-1][3]]) + array([flow2["y"][-1][2], flow2["y"][-1][3]])
# generate primer vector history
p = phistory(dv1, dv2, flow3)
# extract the slopes of the primer vector
dp0 = pslope(flow3, p, 1, 0)
dp1 = pslope(flow3, p, -1, -2)
# scalar cost function
J = dp0 ** 2 + dp1 ** 2
print ("Finished an iteration of opt_pterminal!")
return J, flow1, flow2, flow3