Merge pull request #27 from istellartech/class-ver

Code refactoring
istellartech · Sep 6, 2024 · 7c284fd · 7c284fd
2 parents 537b0b6 + ad32ec7
commit 7c284fd
Show file tree

Hide file tree

Showing 22 changed files with 4,012 additions and 3,471 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,6 +1,8 @@
 __pycache__/*
-examples/*
+example/*
 figures/*
 input/*
 output/*
 *.png
+data/*
+test/*
diff --git a/Trajectory_Optimization.py b/Trajectory_Optimization.py
@@ -31,14 +31,20 @@
 import pandas as pd
 from pyoptsparse import IPOPT, SNOPT, Optimization
 
-from utils import *
-from coordinate import *
-from initialize_output import *
-from constraints import *
-from PSfunctions import *
-from USStandardAtmosphere import *
-
-version = "0.7.3"
+from lib.coordinate import *
+from initialize import initialize_xdict_6DoF_from_file, initialize_xdict_6DoF_2
+from output_result import output_result
+import lib.con_init_terminal_knot as con_a
+import lib.con_trajectory as con_traj
+import lib.con_aero as con_aero
+import lib.con_dynamics as con_dynamics
+import lib.con_waypoint as con_wp
+from user_constraints import equality_user, inequality_user
+import lib.con_user as con_user
+from lib.cost_gradient import cost_6DoF, cost_jac
+from lib.SectionParameters import PSparams
+
+version = "0.7.4"
 
 mission_name = sys.argv[1]
 
@@ -117,18 +123,14 @@
 
 N = sum(nodes)
 
-D = [differentiation_matrix_LGR(n) for n in nodes]
-tau = [nodes_LGR(n) for n in nodes]
 index = [0]
 k = 0
 for n in nodes:
     k += n
     index.append(k)
 
-pdict["ps_params"] = [
-    {"index_start": index[i], "nodes": nodes[i], "D": D[i], "tau": tau[i]}
-    for i in range(num_sections)
-]
+pdict["ps_params"] = PSparams(nodes)
+
 pdict["wind_table"] = wind_table
 pdict["ca_table"] = ca_table
 pdict["N"] = N
@@ -162,6 +164,7 @@
     "t": unit_t,
 }
 
+pdict["dx"] = 1.0e-8
 
 condition = {**settings["TerminalCondition"], **settings["FlightConstraint"]}
 condition["init"] = {}
@@ -191,38 +194,48 @@
 def objfunc(xdict):
     funcs = {}
     funcs["obj"] = cost_6DoF(xdict, condition)
-    funcs["eqcon_init"] = equality_init(xdict, pdict, unitdict, condition)
-    funcs["eqcon_time"] = equality_time(xdict, pdict, unitdict, condition)
-    funcs["eqcon_dyn_mass"] = equality_dynamics_mass(xdict, pdict, unitdict, condition)
-    funcs["eqcon_dyn_pos"] = equality_dynamics_position(
+    funcs["eqcon_init"] = con_a.equality_init(xdict, pdict, unitdict, condition)
+    funcs["eqcon_time"] = con_a.equality_time(xdict, pdict, unitdict, condition)
+    funcs["eqcon_dyn_mass"] = con_dynamics.equality_dynamics_mass(
         xdict, pdict, unitdict, condition
     )
-    funcs["eqcon_dyn_vel"] = equality_dynamics_velocity(
+    funcs["eqcon_dyn_pos"] = con_dynamics.equality_dynamics_position(
         xdict, pdict, unitdict, condition
     )
-    funcs["eqcon_dyn_quat"] = equality_dynamics_quaternion(
+    funcs["eqcon_dyn_vel"] = con_dynamics.equality_dynamics_velocity(
+        xdict, pdict, unitdict, condition
+    )
+    funcs["eqcon_dyn_quat"] = con_dynamics.equality_dynamics_quaternion(
         xdict, pdict, unitdict, condition
     )
 
-    funcs["eqcon_knot"] = equality_knot_LGR(xdict, pdict, unitdict, condition)
-    funcs["eqcon_terminal"] = equality_6DoF_LGR_terminal(
+    funcs["eqcon_knot"] = con_a.equality_knot_LGR(xdict, pdict, unitdict, condition)
+    funcs["eqcon_terminal"] = con_a.equality_6DoF_LGR_terminal(
         xdict, pdict, unitdict, condition
     )
-    funcs["eqcon_rate"] = equality_6DoF_rate(xdict, pdict, unitdict, condition)
-    funcs["eqcon_pos"] = equality_posLLH(xdict, pdict, unitdict, condition)
-    funcs["eqcon_iip"] = equality_IIP(xdict, pdict, unitdict, condition)
-    funcs["eqcon_user"] = equality_user(xdict, pdict, unitdict, condition)
-
-    funcs["ineqcon_alpha"] = inequality_max_alpha(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_q"] = inequality_max_q(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_qalpha"] = inequality_max_qalpha(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_mass"] = inequality_mass(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_kick"] = inequality_kickturn(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_time"] = inequality_time(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_pos"] = inequality_posLLH(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_iip"] = inequality_IIP(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_antenna"] = inequality_antenna(xdict, pdict, unitdict, condition)
-    funcs["ineqcon_user"] = inequality_user(xdict, pdict, unitdict, condition)
+    funcs["eqcon_rate"] = con_traj.equality_6DoF_rate(xdict, pdict, unitdict, condition)
+    funcs["eqcon_pos"] = con_wp.equality_posLLH(xdict, pdict, unitdict, condition)
+    funcs["eqcon_iip"] = con_wp.equality_IIP(xdict, pdict, unitdict, condition)
+    funcs["eqcon_user"] = con_user.equality_user(xdict, pdict, unitdict, condition)
+
+    funcs["ineqcon_alpha"] = con_aero.inequality_max_alpha(
+        xdict, pdict, unitdict, condition
+    )
+    funcs["ineqcon_q"] = con_aero.inequality_max_q(xdict, pdict, unitdict, condition)
+    funcs["ineqcon_qalpha"] = con_aero.inequality_max_qalpha(
+        xdict, pdict, unitdict, condition
+    )
+    funcs["ineqcon_mass"] = con_traj.inequality_mass(xdict, pdict, unitdict, condition)
+    funcs["ineqcon_kick"] = con_traj.inequality_kickturn(
+        xdict, pdict, unitdict, condition
+    )
+    funcs["ineqcon_time"] = con_a.inequality_time(xdict, pdict, unitdict, condition)
+    funcs["ineqcon_pos"] = con_wp.inequality_posLLH(xdict, pdict, unitdict, condition)
+    funcs["ineqcon_iip"] = con_wp.inequality_IIP(xdict, pdict, unitdict, condition)
+    funcs["ineqcon_antenna"] = con_wp.inequality_antenna(
+        xdict, pdict, unitdict, condition
+    )
+    funcs["ineqcon_user"] = con_user.inequality_user(xdict, pdict, unitdict, condition)
 
     fail = False
 
@@ -232,48 +245,68 @@ def objfunc(xdict):
 def sens(xdict, funcs):
     funcsSens = {}
     funcsSens["obj"] = cost_jac(xdict, condition)
-    funcsSens["eqcon_init"] = equality_jac_init(xdict, pdict, unitdict, condition)
-    funcsSens["eqcon_time"] = equality_jac_time(xdict, pdict, unitdict, condition)
-    funcsSens["eqcon_dyn_mass"] = equality_jac_dynamics_mass(
+    funcsSens["eqcon_init"] = con_a.equality_jac_init(xdict, pdict, unitdict, condition)
+    funcsSens["eqcon_time"] = con_a.equality_jac_time(xdict, pdict, unitdict, condition)
+    funcsSens["eqcon_dyn_mass"] = con_dynamics.equality_jac_dynamics_mass(
         xdict, pdict, unitdict, condition
     )
-    funcsSens["eqcon_dyn_pos"] = equality_jac_dynamics_position(
+    funcsSens["eqcon_dyn_pos"] = con_dynamics.equality_jac_dynamics_position(
         xdict, pdict, unitdict, condition
     )
-    funcsSens["eqcon_dyn_vel"] = equality_jac_dynamics_velocity(
+    funcsSens["eqcon_dyn_vel"] = con_dynamics.equality_jac_dynamics_velocity(
         xdict, pdict, unitdict, condition
     )
-    funcsSens["eqcon_dyn_quat"] = equality_jac_dynamics_quaternion(
+    funcsSens["eqcon_dyn_quat"] = con_dynamics.equality_jac_dynamics_quaternion(
         xdict, pdict, unitdict, condition
     )
 
-    funcsSens["eqcon_knot"] = equality_jac_knot_LGR(xdict, pdict, unitdict, condition)
-    funcsSens["eqcon_terminal"] = equality_jac_6DoF_LGR_terminal(
+    funcsSens["eqcon_knot"] = con_a.equality_jac_knot_LGR(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["eqcon_terminal"] = con_a.equality_jac_6DoF_LGR_terminal(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["eqcon_rate"] = con_traj.equality_jac_6DoF_rate(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["eqcon_pos"] = con_wp.equality_jac_posLLH(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["eqcon_iip"] = con_wp.equality_jac_IIP(xdict, pdict, unitdict, condition)
+    funcsSens["eqcon_user"] = con_user.equality_jac_user(
         xdict, pdict, unitdict, condition
     )
-    funcsSens["eqcon_rate"] = equality_jac_6DoF_rate(xdict, pdict, unitdict, condition)
-    funcsSens["eqcon_pos"] = equality_jac_posLLH(xdict, pdict, unitdict, condition)
-    funcsSens["eqcon_iip"] = equality_jac_IIP(xdict, pdict, unitdict, condition)
-    funcsSens["eqcon_user"] = equality_jac_user(xdict, pdict, unitdict, condition)
 
-    funcsSens["ineqcon_alpha"] = inequality_jac_max_alpha(
+    funcsSens["ineqcon_alpha"] = con_aero.inequality_jac_max_alpha(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["ineqcon_q"] = con_aero.inequality_jac_max_q(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["ineqcon_qalpha"] = con_aero.inequality_jac_max_qalpha(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["ineqcon_mass"] = con_traj.inequality_jac_mass(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["ineqcon_kick"] = con_traj.inequality_jac_kickturn(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["ineqcon_time"] = con_a.inequality_jac_time(
+        xdict, pdict, unitdict, condition
+    )
+    funcsSens["ineqcon_pos"] = con_wp.inequality_jac_posLLH(
         xdict, pdict, unitdict, condition
     )
-    funcsSens["ineqcon_q"] = inequality_jac_max_q(xdict, pdict, unitdict, condition)
-    funcsSens["ineqcon_qalpha"] = inequality_jac_max_qalpha(
+    funcsSens["ineqcon_iip"] = con_wp.inequality_jac_IIP(
         xdict, pdict, unitdict, condition
     )
-    funcsSens["ineqcon_mass"] = inequality_jac_mass(xdict, pdict, unitdict, condition)
-    funcsSens["ineqcon_kick"] = inequality_jac_kickturn(
+    funcsSens["ineqcon_antenna"] = con_wp.inequality_jac_antenna(
         xdict, pdict, unitdict, condition
     )
-    funcsSens["ineqcon_time"] = inequality_jac_time(xdict, pdict, unitdict, condition)
-    funcsSens["ineqcon_pos"] = inequality_jac_posLLH(xdict, pdict, unitdict, condition)
-    funcsSens["ineqcon_iip"] = inequality_jac_IIP(xdict, pdict, unitdict, condition)
-    funcsSens["ineqcon_antenna"] = inequality_jac_antenna(
+    funcsSens["ineqcon_user"] = con_user.inequality_jac_user(
         xdict, pdict, unitdict, condition
     )
-    funcsSens["ineqcon_user"] = inequality_jac_user(xdict, pdict, unitdict, condition)
 
     fail = False
     return funcsSens, fail
@@ -446,12 +479,11 @@ def sens(xdict, funcs):
 for i in range(num_sections):
     to = sol.xStar["t"][i]
     tf = sol.xStar["t"][i + 1]
-    tau_x = np.hstack((-1.0, pdict["ps_params"][i]["tau"]))
+    tau_x = np.hstack((-1.0, pdict["ps_params"].tau(i)))
     tu_res = np.hstack(
         (
             tu_res,
-            (pdict["ps_params"][i]["tau"] * (tf - to) / 2 + (tf + to) / 2)
-            * unitdict["t"],
+            (pdict["ps_params"].tau(i) * (tf - to) / 2 + (tf + to) / 2) * unitdict["t"],
         )
     )
     tx_res = np.hstack(
@@ -493,6 +525,6 @@ def sens(xdict, funcs):
 with open("output/{}-optResult.txt".format(settings["name"]), mode="w") as fout:
     fout.write("".join(res_info))
 
-out = output_6DoF(sol.xStar, unitdict, tx_res, tu_res, pdict)
+out = output_result(sol.xStar, unitdict, tx_res, tu_res, pdict)
 
 out.to_csv("output/{}-trajectoryResult.csv".format(settings["name"]), index=False)
diff --git a/_user_constraints_empty.py b/_user_constraints_empty.py
@@ -23,11 +23,12 @@
 # SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 #
 
+
 def equality_user(xdict, pdict, unitdict, condition):
 
     return None
 
 
 def inequality_user(xdict, pdict, unitdict, condition):
 
-    return None
+    return None