fix: Исправлено смыкание на идеальной траектории

src/OptAlgorithm/OptTimeCalculator.py

@@ -22,7 +22,7 @@ class OptTimeCalculator(AutoConfigClass):
         t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 /2)) / v0)
         T1 = t1 + t2t
         
-        t21 = sqrt(h2/self.a_max_2)
+        t21 = sqrt(h2/(self.a_max_2))
         t21 = min(self.v_max_2/self.a_max_2, t21)
         t22 = max(0, (h2 - (self.a_max_2 * t21 * t21)) / self.v_max_2)
         T2 = t22 + 2 * t21
@@ -207,26 +207,26 @@ class OptTimeCalculator(AutoConfigClass):
         v0q = min(sqrt(2 * self.a_max_1 * s), self.v_max_1)
         v0 = min(v0q, sqrt(1/(self.k_hardness_1*self.mass_1))* self.Ftogrow)
         t1 = T - sqrt(max(0, T**2 - 2 * s / self.a_max_1))
-        t1 = min(t1, v0 / self.a_max_1)
+        assert t1 < v0 / self.a_max_1
         t2 = max(0, (s - self.a_max_1*t1**2/2) / (self.a_max_1*t1))
         return t1, t2
     
     def calcFirstOpen(self, T : float, s : float) -> tuple[float, float]:
         t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_1)) / 2
-        t1 = min(t1, self.v_max_1 / self.a_max_1)
+        assert t1 < self.v_max_1 / self.a_max_1
         t2 = max(0, (s - self.a_max_1*t1**2/2) / (self.a_max_1*t1))
         return t1, t2
         
     def calcSecondOpen(self, T : float, s : float) -> tuple[float, float]:
         t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_2)) / 2
-        t1 = min(t1, self.v_max_2 / self.a_max_2)
+        assert t1 < self.v_max_2 / self.a_max_2
         t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1))
         return t1, t2
     
     def calcSecondClose(self, T : float, s : float) -> tuple[float, float]:
         t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_2)) / 2
-        t1 = min(t1, self.v_max_2 / self.a_max_2)
-        t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1))
+        assert t1 < self.v_max_2 / self.a_max_2
+        t2 = max(0, (s - self.a_max_2*t1**2) / (self.a_max_2*t1))
         return t1, t2
     
     def calcSecondOpenOffset(self, t1 : float, t2 : float, sq : float) -> float:

src/testAlgo.py

@@ -0,0 +1,43 @@
+from src.OptAlgorithm.OptAlgorithm import OptAlgorithm
+from src.utils import read_json 
+
+from matplotlib import pyplot as plt, use
+
+from numpy import cos, sin, sqrt, cbrt, arcsin, linspace, array      
+        
+if __name__ == "__main__":
+    
+    tq = 1
+    ts = linspace(0, tq, 200000)
+    
+    operator_params = read_json("params/operator_params.json")
+    system_params = read_json("params/system_params.json")
+    
+    non_array_operator_params = {}
+    i = 1
+    for key, value in operator_params.items():
+        if hasattr(value, "__len__"):
+            if len(value) > i:
+                non_array_operator_params[key] = value[i]
+            else:
+                non_array_operator_params[key] = value[0]
+        else:
+            non_array_operator_params[key] = value
+            
+    non_array_system_params = {}
+    for key, value in system_params.items():
+        if hasattr(value, "__len__"):
+            if len(value) > i:
+                non_array_system_params[key] = value[i]
+            else:
+                non_array_system_params[key] = value[0]
+        else:
+            non_array_system_params[key] = value
+    
+    
+    opt = OptAlgorithm(non_array_operator_params, non_array_system_params)
+    Xs = array([opt.getVar("X2", t) for t in ts])
+
+    
+    plt.plot(ts, Xs)
+    plt.show()