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

src/OptAlgorithm/OptTimeCalculator.py

@@ -3,14 +3,16 @@ from numpy import sqrt, arcsin, arccos, cos, sin
 from OptAlgorithm.AutoConfigClass import AutoConfigClass
 from OptAlgorithm.ConstantCalculator import ConstantCalculator
 
-class OptTimeCalculator(AutoConfigClass):
 
+class OptTimeCalculator(AutoConfigClass):
     params_list = []
+
     def __init__(self, operator_config: dict, system_config: dict):
 
         cCalculator = ConstantCalculator(operator_config, system_config)
         super().__init__(OptTimeCalculator.params_list, operator_config, system_config, cCalculator.calc())
         self.allTimes = {}
+        self.check_eps = 1e-7
 
     def tGrowNominal(self, F: float) -> float:
         return arcsin(F / (self.Ftogrow)) * sqrt(self.mass_1 / self.k_hardness_1)
@@ -22,7 +24,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
@@ -53,7 +55,6 @@ class OptTimeCalculator(AutoConfigClass):
 
         topen_1_acc, topen_1_speed = self.calcFirstOpen(T1, l1 + Fs1)
         offset = self.calcSecondOpenOffset(topen_1_acc, topen_1_speed, Fs1)
-        
         topen_2_acc, topen_2_speed = self.calcSecondOpen(T2 - offset, l2)
 
         self.allTimes["topen_1_acc"] = topen_1_acc
@@ -104,8 +105,10 @@ class OptTimeCalculator(AutoConfigClass):
         vFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1 / (self.mass_1)) * self.Ftogrow)
 
         L = sqrt(self.k_hardness_1 / self.mass_1 * self.eff_control ** 2 + vF0 * vF0)
-        tspeed = sqrt(self.mass_1/self.k_hardness_1) * (arcsin(vFmax / L) - arccos(sqrt(self.k_hardness_1 / self.mass_1) * self.eff_control / L))
+        tspeed = sqrt(self.mass_1 / self.k_hardness_1) * (
-        Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1/self.freq * vF0 * sin(self.freq * tspeed)
+                    arcsin(vFmax / L) - arccos(sqrt(self.k_hardness_1 / self.mass_1) * self.eff_control / L))
+        Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1 / self.freq * vF0 * sin(
+            self.freq * tspeed)
         eps = 1e1
         if self.freq ** 2 * self.Ftogrow ** 2 - vFmax ** 2 < -eps:
             raise Exception("""Номинальная траектория набора усилия не может быть достигнута, максимальная скорость превысила скорость траектории
@@ -133,7 +136,7 @@ class OptTimeCalculator(AutoConfigClass):
         self.Topen(s1, s2, l1, l2, self.force_target / self.k_hardness_1, 0)
         return self.allTimes
 
-    def Tmovement(self, closeAlgo, tmark) -> None:
+    def Tmovement(self, closeAlgo, tmark) -> tuple[list, list]:
         contact = [self.contact_distance_1, self.contact_distance_2]
         v0s = []
         pos0s = []
@@ -149,7 +152,6 @@ class OptTimeCalculator(AutoConfigClass):
             pos0s.append(closeAlgo("X" + str(i), "Open", tmark))
             Tfull = self.time_robot_movement
 
-            
             L = self.__dict__["distance_l_" + str(i)]
             maxL = contact[i - 1] - L - x0
             self.Tmovementi(i, x, Tfull, v0, maxL)
@@ -161,7 +163,8 @@ class OptTimeCalculator(AutoConfigClass):
         a = self.__dict__["a_max_" + str(i)]
         t3 = (Tfull + v0 / a) / 2
 
-        sqrtval = a**2 * (a**2 * (Tfull+2*t3)**2 - 8 * a * Sfull + 2 * a* v0 * (Tfull+2*t3) - 3 *v0**2)
+        sqrtval = a ** 2 * (
+                    a ** 2 * (Tfull + 2 * t3) ** 2 - 8 * a * Sfull + 2 * a * v0 * (Tfull + 2 * t3) - 3 * v0 ** 2)
         if sqrtval < 0:
             raise Exception("""Невозможно с S_{i} добраться но H*_{i} за указанное время, 
                             проверьте distance_s_{i}, distance_h_end{i}, time_command, time_robot_movement""")
@@ -207,26 +210,30 @@ 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)
+        if t1 > v0/ self.a_max_1 + self.check_eps:
+            raise Exception("""Мы вышли за границы разгона - смыкание FE, вообще не знаю как так получилось""")
         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)
+        if t1 > self.v_max_1 / self.a_max_1 + self.check_eps:
+            raise Exception("""Мы вышли за границы разгона - раскрытие FE, вообще не знаю как так получилось""")
         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)
+        if t1 > self.v_max_2 / self.a_max_2 + self.check_eps:
-        t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1))
+            raise Exception("""Мы вышли за границы разгона - раскрытие ME, вообще не знаю как так получилось""")
+        t2 = max(0, (s - self.a_max_2 * t1 ** 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)
+        if t1 > self.v_max_2 / self.a_max_2 + self.check_eps:
-        t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1))
+            raise Exception("""Мы вышли за границы разгона - смыкание ME, вообще не знаю как так получилось""")
+        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("X1", t) for t in ts])
+
+    
+    plt.plot(ts, Xs)
+    plt.show()