# Guess.plot(prob.time_all_section, M_init, "Mass", "time", "Mass")
# if(flag_savefig):plt.savefig(savefig_file + "guess_mass" + ".png")
# thrust profile
T_init = Guess.cubic(prob.time_all_section, 3.5, 0.0, 0.0, 0.0)
# Guess.plot(prob.time_all_section, T_init, "Thrust Guess", "time", "Thrust")
# if(flag_savefig):plt.savefig(savefig_file + "guess_thrust" + ".png")
plt.show()
# ========================
# Substitution initial value to parameter vector to be optimized
prob.set_states_all_section(0, H_init)
prob.set_states_all_section(1, V_init)
prob.set_states_all_section(2, M_init)
prob.set_controls_all_section(0, T_init)
# ========================
# Main Process
# Assign problem to SQP solver
prob.dynamics = [dynamics]
prob.knot_states_smooth = []
prob.cost = cost
prob.cost_derivative = None
prob.equality = equality
prob.inequality = inequality
def display_func():
h = prob.states_all_section(0)
print("max altitude: {0:.5f}".format(h[-1]))
theta_init = Guess.linear(prob.time_all_section, np.deg2rad(30),
np.deg2rad(30))
# Guess.plot(prob.time_all_section, theta_init, "gamma", "time", "gamma")
# if(flag_savefig):plt.savefig(savefig_dir + "guess_gamma" + savefig_add + ".png")
x_init = Guess.linear(prob.time_all_section, 0.0, obj.l)
# Guess.plot(prob.time_all_section, x_init, "x", "time", "x")
# if(flag_savefig):plt.savefig(savefig_dir + "guess_x" + savefig_add + ".png")
y_init = Guess.linear(prob.time_all_section, 0.0, obj.l / np.sqrt(3))
# Guess.plot(prob.time_all_section, theta_init, "y", "time", "y")
# if(flag_savefig):plt.savefig(savefig_dir + "guess_y" + savefig_add + ".png")
prob.set_states_all_section(0, x_init)
prob.set_states_all_section(1, y_init)
prob.set_controls_all_section(0, theta_init)
# ========================
# Main Process
# Assign problem to SQP solver
prob.dynamics = [dynamics]
prob.knot_states_smooth = []
prob.cost = cost
prob.cost_derivative = cost_derivative
prob.equality = equality
prob.inequality = inequality
prob.solve(obj)
# ========================
# Post Process