add waypoint position constraints(temp)

istellartech · Nov 11, 2023 · abc0a1e · abc0a1e
1 parent 9ba15c0
commit abc0a1e
Show file tree

Hide file tree

Showing 3 changed files with 127 additions and 0 deletions.
diff --git a/Trajectory_Optimization.py b/Trajectory_Optimization.py
@@ -209,6 +209,7 @@ def objfunc(xdict):
         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)
 
@@ -218,6 +219,7 @@ def objfunc(xdict):
     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)
@@ -250,6 +252,7 @@ def sens(xdict, funcs):
         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)
 
@@ -265,6 +268,7 @@ def sens(xdict, funcs):
         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(
         xdict, pdict, unitdict, condition
@@ -328,6 +332,7 @@ def sens(xdict, funcs):
     "eqcon_knot": ["mass", "position", "velocity", "quaternion"],
     "eqcon_terminal": ["position", "velocity"],
     "eqcon_rate": ["position", "quaternion", "u"],
+    "eqcon_pos": ["position", "t"],
     "eqcon_iip": ["position", "velocity", "t"],
     "eqcon_user": ["mass", "position", "velocity", "quaternion", "u", "t"],
     "ineqcon_alpha": ["position", "velocity", "quaternion", "t"],
@@ -336,6 +341,7 @@ def sens(xdict, funcs):
     "ineqcon_mass": ["mass"],
     "ineqcon_kick": ["u"],
     "ineqcon_time": ["t"],
+    "ineqcon_pos": ["position", "t"],
     "ineqcon_iip": ["position", "velocity", "t"],
     "ineqcon_antenna": ["position", "t"],
     "ineqcon_user": ["mass", "position", "velocity", "quaternion", "u", "t"],

diff --git a/constraints.py b/constraints.py
@@ -2293,6 +2293,124 @@ def inequality_jac_IIP(xdict, pdict, unitdict, condition):
         return None
 
 
+def equality_posLLH(xdict, pdict, unitdict, condition):
+    """Equality constraint about IIP position."""
+    con = []
+    unit_pos = unitdict["position"]
+    unit_t = unitdict["t"]
+
+    pos_ = xdict["position"].reshape(-1, 3)
+
+    t = xdict["t"]
+
+    num_sections = pdict["num_sections"]
+
+    if "waypoint" not in condition:
+        return None
+
+    for i in range(num_sections - 1):
+
+        section_name = pdict["params"][i]["name"]
+        if section_name in condition["waypoint"]:
+
+            waypoint = condition["waypoint"][section_name]
+            a = pdict["ps_params"][i]["index_start"]
+            pos = pos_[a + i] * unit_pos
+            to = t[i] * unit_t
+            posLLH = eci2geodetic(pos, to)
+            lon_origin = pdict["LaunchCondition"]["lon"]
+            lat_origin = pdict["LaunchCondition"]["lat"]
+            downrange = haversine(
+                lon_origin, lat_origin, posLLH[1], posLLH[0], 6378137.0
+            )
+
+            # altitude
+            if "altitude" in waypoint:
+                if "exact" in waypoint["altitude"]:
+                    con.append((posLLH[2] / waypoint["altitude"]["exact"]) - 1.0)
+
+            # downrange
+            if "downrange" in waypoint:
+                if "exact" in waypoint["downrange"]:
+                    con.append((downrange / waypoint["downrange"]["exact"]) - 1.0)
+
+    if len(con) == 0:
+        return None
+    else:
+        return np.concatenate(con, axis=None)
+
+
+def equality_jac_posLLH(xdict, pdict, unitdict, condition):
+    """Jacobian of equality_posLLH."""
+    if equality_posLLH(xdict, pdict, unitdict, condition) is not None:
+        return jac_fd(equality_posLLH, xdict, pdict, unitdict, condition)
+    else:
+        return None
+
+
+def inequality_posLLH(xdict, pdict, unitdict, condition):
+    """Inequality constraint about IIP position."""
+    con = []
+    unit_pos = unitdict["position"]
+    unit_t = unitdict["t"]
+
+    pos_ = xdict["position"].reshape(-1, 3)
+
+    t = xdict["t"]
+
+    num_sections = pdict["num_sections"]
+
+    if "waypoint" not in condition:
+        return None
+
+    for i in range(num_sections - 1):
+
+        section_name = pdict["params"][i]["name"]
+        if section_name in condition["waypoint"]:
+
+            waypoint = condition["waypoint"][section_name]
+            a = pdict["ps_params"][i]["index_start"]
+            pos = pos_[a + i] * unit_pos
+            to = t[i] * unit_t
+            posLLH = eci2geodetic(pos, to)
+            lon_origin = pdict["LaunchCondition"]["lon"]
+            lat_origin = pdict["LaunchCondition"]["lat"]
+            downrange = haversine(
+                lon_origin, lat_origin, posLLH[1], posLLH[0], 6378137.0
+            )
+
+            # altitude
+            if "altitude" in waypoint:
+                # min
+                if "min" in waypoint["altitude"]:
+                    con.append((posLLH[2] / waypoint["altitude"]["min"]) - 1.0)
+                # max
+                if "max" in waypoint["altitude"]:
+                    con.append(-(posLLH[2] / waypoint["altitude"]["max"]) + 1.0)
+
+            # downrange
+            if "downrange" in waypoint:
+                # min
+                if "min" in waypoint["downrange"]:
+                    con.append((downrange / waypoint["downrange"]["min"]) - 1.0)
+                # max
+                if "max" in waypoint["downrange"]:
+                    con.append(-(downrange / waypoint["downrange"]["min"]) + 1.0)
+
+    if len(con) == 0:
+        return None
+    else:
+        return np.concatenate(con, axis=None)
+
+
+def inequality_jac_posLLH(xdict, pdict, unitdict, condition):
+    """Jacobian of inequality_posLLH."""
+    if inequality_posLLH(xdict, pdict, unitdict, condition) is not None:
+        return jac_fd(inequality_posLLH, xdict, pdict, unitdict, condition)
+    else:
+        return None
+
+
 def equality_jac_user(xdict, pdict, unitdict, condition):
     """Jacobian of user-defined equality constraint."""
     if equality_user(xdict, pdict, unitdict, condition) is not None:

diff --git a/example/example-settings.json b/example/example-settings.json
@@ -69,6 +69,9 @@
     },
     "waypoint": {
       "FAIRING": {
+        "altitude": {
+          "exact": 100000.0
+        },
         "lon_IIP" : {
           "min": 145.0
         }