def mee_ocp_central_difference(x):
"""
Central different computation of the Jacobian of the cost funtion.
Parameters:
===========
tspan -- vector containting initial and final time
p0 -- initial costates guess
states0 -- initial state vector (modified equinoctial elements)
rho -- switch smoothing parameter
eclipse -- boolean (true or false)
Returns:
========
Jac -- Jacobian
External:
=========
numpy, spipy.integrate
"""
# TEMP
tspan = np.array([0, 642.54])
rho = 1
eclipse = False
meef = np.array([6.610864583184714, 0.0, 0.0, 0.0, 0.0, 0.0])
# Initialization
IC1 = np.zeros(14)
IC2 = np.zeros(14)
grad = np.zeros(7)
DEL = 1e-5 * (x[7:14] / np.linalg.norm(x[7:14]))
for i in range(0, 7):
IC1 = x
IC2 = x
IC1[i + 7] = IC1[i + 7] + DEL[i]
IC2[i + 7] = IC2[i + 7] - DEL[i]
# Integrate Dynamics
sol1 = integrator(
lambda t, y: eom_mee_twobodyJ2_minfuel(t, y, rho, eclipse),
tspan,
IC1,
method='LSODA',
rtol=1e-13)
sol2 = integrator(
lambda t, y: eom_mee_twobodyJ2_minfuel(t, y, rho, eclipse),
tspan,
IC2,
method='LSODA',
rtol=1e-13)
res1 = np.linalg.norm(meef[0:5] - sol1.y[0:5, -1])
res2 = np.linalg.norm(meef[0:5] - sol2.y[0:5, -1])
grad[i] = (res1 - res2) / (2 * DEL[i])
return grad
def residuals_mee_ocp(p0, tspan, mee0, meef, rho, eclipse):
"""
Computes error between current final state and desired final state for the
low thrust, min-fuel, optimal trajectory using modified equinoctial elements.
Parameters:
===========
p0 -- current initial costate
tspan -- vector containing initial and final time
mee0 -- initial states (modified equinoctial elements)
meef -- final states (modified equinoctial elements)
rho -- switch smoothing parameter
Returns:
========
res -- residual
External:
=========
numpy, scipy.integrate
"""
## Computes the error between the current and desired final state.
states_in = np.zeros(14)
states_in[0:7] = mee0[0:7]
states_in[7:14] = p0[0:7]
# sol = integrator(eom_mee_twobody_minfuel,(tspan[0],tspan[-1]),states_in,method='LSODA',rtol=1e-12)
sol = integrator(
lambda t, y: eom_mee_twobodyJ2_minfuel(t, y, rho, eclipse),
tspan,
states_in,
method='LSODA',
rtol=1e-12)
# Free final mass and free final true longitude (angle)
res = np.linalg.norm(meef[0:5] - sol.y[0:5, -1])
print(res)
return res
def residuals(p0):
# Residual function
# TEMP: For now I have copied these parameters into the function to make it more inline with the Rosenbrock
# function above, but eventually we'll need to pass them as input arguments
tspan = np.array([0, 642.54])
rho = 1
eclipse = False
mee0 = np.array(
[1.8225634499541166, 0.725, 0.0, 0.06116262015048431, 0.0, 0, 100])
meef = np.array([6.610864583184714, 0.0, 0.0, 0.0, 0.0, 0.0])
## Computes the error between the current and desired final state.
states_in = np.zeros(14)
states_in[0:7] = mee0[0:7]
states_in[7:14] = p0[0:7]
# sol = integrator(eom_mee_twobody_minfuel,(tspan[0],tspan[-1]),states_in,method='LSODA',rtol=1e-12)
sol = integrator(
lambda t, y: eom_mee_twobodyJ2_minfuel(t, y, rho, eclipse),
tspan,
states_in,
method='LSODA',
rtol=1e-12)
# Free final mass and free final true longitude (angle)
res = np.linalg.norm(meef[0:5] - sol.y[0:5, -1])
print(res)
return res
def mee_ocp_central_difference(states0,tspan,p0,rho,eclipse):
"""
Central different computation of the Jacobian of the cost funtion.
Parameters:
===========
tspan -- vector containting initial and final time
p0 -- initial costates guess
states0 -- initial state vector (modified equinoctial elements)
rho -- switch smoothing parameter
eclipse -- boolean (true or false)
Returns:
========
Jac -- Jacobian
External:
=========
numpy, spipy.integrate
"""
# Sizes
sz1 = np.size(states0)
sz2 = sz1 - 2 # Number of fixed final states
# Initialization
IC = np.zeros(2*sz1)
Jac = np.zeros((sz2,sz1))
Del_plus = np.zeros((sz2,sz1))
Del_minus = np.zeros((sz2,sz1))
DEL = 1e-5*(p0/np.linalg.norm(p0))
for tcnt in range(0,2*sz1):
if (tcnt < sz1):
IC[0:sz1] = states0[0:sz1]
IC[0:sz1] = p0[0:sz1]
IC[tcnt+sz1] = IC[tcnt+sz1]+DEL[tcnt]
elif (tcnt >= sz1):
IC[0:sz1] = states0[0:sz1]
IC[sz1:2*sz1] = p0[0:sz1]
IC[tcnt] = IC[tcnt]-DEL[tcnt-sz1]
# Integrate Dynamics
sol = integrator(lambda t,y: eom_mee_twobodyJ2_minfuel(t,y,rho,eclipse),tspan,IC,method='LSODA',rtol=1e-13)
soln = sol.y[0:sz1,-1]
if (tcnt < sz1):
Del_plus[0:sz2,tcnt-1] = soln[0:sz2] # Free final mass & free final true longitude (angle)
elif (tcnt >= sz1):
Del_minus[0:sz2,tcnt-sz1] = soln[0:sz2] # Free final mass & free final true longitude (angle)
# Compute Jacobian (central differences)
for i in range(sz2):
for j in range(sz1):
Jac[i,j] = (Del_plus[i,j] - Del_minus[i,j])/(2*DEL[j])
return Jac
[soln, p0, mps_err] = mps_mee_ocp(tspan, p0, mee0, meef, rho, eclipse,
mps_tol)
print("rho:", rho, "mps err:", mps_err)
rho = rho * rho_rate
# Update
iter = iter + 1
# Final Solution
rho = rho / rho_rate # rho for converged solution
mee0_new = np.zeros(14)
mee0_new[0:7] = mee0[0:7]
mee0_new[7:14] = p0[0:7]
# Propagation Solution
sol = integrator(lambda t, y: eom_mee_twobodyJ2_minfuel(t, y, rho, eclipse),
tspan,
mee0_new,
method='LSODA',
rtol=1e-13)
data = np.array(soln.y, dtype=float).T
time = np.array(soln.t, dtype=float).T
# Plot Solution
plot_mee_minfuel(time, data, rho, eclipse)
# Save Solution
df = pd.DataFrame(np.array([time, data]).T, columns=['time', 'trajectory'])
df.to_csv("output_soln.csv", index=False)
# Save Solution Parameters
df = pd.DataFrame(np.array([rho, eclipse, sc.Isp, sc.A, sc.eff]).T,
def mps_mee_ocp(tspan, p0, states0, statesf, rho, eclipse, mps_tol):
"""
Shooting method to solve min-fuel optimal control prolem
Parameters:
===========
tspan -- vector containing initial and final time
p0 -- initial costates guess
states0 -- initial state vector (modified equinoctial elements)
statesf -- final state vector (modified equinoctial elements)
rho -- switch smoothing parameter
eclipse -- boolean (true or false)
mps_tol -- user specified convergence tolerance
Returns:
========
traj -- optimal trajectory
p0 -- initial costate for optimal trajectory
mps_err -- shooting method convergence error
External:
=========
numpy, const.py, spipy.integrate
"""
mps_err = 1
cnt = 0
IC = np.zeros(14)
Del = np.zeros((5, 7))
Del_plus = np.zeros((5, 7))
Del_minus = np.zeros((5, 7))
DEL = np.array([1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5])
# p0 = states0[7:14]
MatInv = np.zeros((7, 5))
res = np.zeros(7)
del_p0 = np.zeros(7)
while mps_err > mps_tol and cnt < 30:
for tcnt in range(0, 15):
if tcnt == 0:
IC[0:7] = states0[0:7]
IC[7:14] = p0[0:7]
elif tcnt > 0 and tcnt < 8:
IC[0:7] = states0[0:7]
IC[7:14] = p0[0:7]
IC[tcnt + 6] = IC[tcnt + 6] + DEL[tcnt - 1]
elif tcnt > 7 and tcnt < 15:
IC[0:7] = states0[0:7]
IC[7:14] = p0[0:7]
IC[tcnt + 6 - 7] = IC[tcnt + 6 - 7] - DEL[tcnt - 1 - 7]
# Integrate Dynamics
sol = integrator(
lambda t, y: eom_mee_twobodyJ2_minfuel(t, y, rho, eclipse),
tspan,
IC,
method='LSODA',
rtol=1e-13)
# sol = integrator(eom_mee_twobody_minfuel,(tspan[0],tspan[-1]),IC,method='LSODA',rtol=1e-12)
soln = sol.y[0:7, -1]
if tcnt == 0:
ref = soln
traj = sol
elif tcnt > 0 and tcnt < 8:
# Del_plus[0:5,tcnt-1] = (soln[0:5]-ref[0:5])/DEL # Free final mass & free final true longitude (angle)
Del_plus[0:5, tcnt - 1] = soln[
0:5] # Free final mass & free final true longitude (angle)
elif tcnt > 7 and tcnt < 15:
Del_minus[0:5, tcnt - 1 - 7] = soln[
0:5] # Free final mass & free final true longitude (angle)
# Compute Updated Variables
for i in range(5):
for j in range(7):
Del[i, j] = (Del_plus[i, j] - Del_minus[i, j]) / (2 * DEL[j])
MatInv = np.linalg.pinv(Del)
res = statesf[0:5] - ref[
0:5] # Free final mass & free final true longitude (angle)
del_p0 = np.matmul(MatInv, res)
p0 = p0 + del_p0
DEL = 1e-5 * (p0 / np.linalg.norm(p0))
# Compute Error
mps_err = np.linalg.norm(res)
print(mps_err)
# Counter
cnt = cnt + 1
return traj, p0, mps_err
def mee_ocp_central_difference(p0):
"""
Central different computation of the Jacobian of the cost funtion.
Parameters:
===========
tspan -- vector containting initial and final time
p0 -- initial costates guess
states0 -- initial state vector (modified equinoctial elements)
rho -- switch smoothing parameter
eclipse -- boolean (true or false)
Returns:
========
Jac -- Jacobian
External:
=========
numpy, spipy.integrate
"""
# TEMP
tspan = np.array([0, 642.54])
rho = 1
eclipse = False
states0 = np.array(
[1.8225634499541166, 0.725, 0.0, 0.06116262015048431, 0.0, 0, 100])
statesf = np.array([6.610864583184714, 0.0, 0.0, 0.0, 0.0, 0.0])
# Initialization
IC = np.zeros(14)
Jac = np.zeros((14, 14))
Del_plus = np.zeros((7, 7))
Del_minus = np.zeros((7, 7))
DEL = 1e-5 * (p0 / np.linalg.norm(p0))
# DEL = np.array([1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5])
for tcnt in range(0, 14):
print(tcnt)
if (tcnt < 7):
IC[0:7] = states0[0:7]
IC[7:14] = p0[0:7]
IC[tcnt + 7] = IC[tcnt + 7] + DEL[tcnt]
elif (tcnt >= 7):
IC[0:7] = states0[0:7]
IC[7:14] = p0[0:7]
IC[tcnt] = IC[tcnt] - DEL[tcnt - 7]
# Integrate Dynamics
sol = integrator(
lambda t, y: eom_mee_twobodyJ2_minfuel(t, y, rho, eclipse),
tspan,
IC,
method='LSODA',
rtol=1e-12)
soln = sol.y[0:7, -1]
if (tcnt < 7):
Del_plus[0:7, tcnt] = soln[
0:7] # Free final mass & free final true longitude (angle)
elif (tcnt >= 7):
Del_minus[0:7, tcnt - 7] = soln[
0:7] # Free final mass & free final true longitude (angle)
# Compute Jacobian (central differences)
for i in range(7):
for j in range(7):
Jac[i + 7, j] = (Del_plus[i, j] - Del_minus[i, j]) / (2 * DEL[j])
# print(Jac)
# sys.exit()
# MatInv = np.linalg.pinv(Del)
# res = statesf[0:5] - ref[0:5] # Free final mass & free final true longitude (angle)
# del_p0 = np.matmul(MatInv,res)
return Jac