feat: Введен альтернативный алгоритм для перемещения, исправлены места ошибок

params/operator_params.json

@@ -34,7 +34,7 @@
     "distance_s_1": [
         0.001,
         0.001,
-        0.001,
+        0.003,
         0.001,
         0.001,
         0.001,
@@ -64,7 +64,7 @@
         0.001
     ],
     "distance_l_1": [
-        0.005,
+        0.0105,
         0.005,
         0.005,
         0.005,
@@ -129,7 +129,7 @@
     ],
     "object_position": [
         0.02743,
-        0.0249,
+        0.016,
         0.024,
         0.02425,
         0.0225,
@@ -176,7 +176,7 @@
         0.0
     ],
     "time_robot_movement": [
-        0.314,
+        0.5,
         0.331,
         0.356,
         0.428,
@@ -189,14 +189,14 @@
         0.44,
         0.425,
         0.464,
-        0.5
+        0.2
     ],
     "object_thickness": [
         0.0045,
         0.0045,
         0.0045,
         0.0045,
-        0.0045,
+        0.0085,
         0.0045,
         0.0045,
         0.0045,
@@ -213,9 +213,9 @@
         5000.0,
         5000.0,
         5000.0,
+        7000.0,
         5000.0,
-        5000.0,
+        2000.0,
-        5000.0,
         5000.0,
         5000.0,
         5000.0,

params/system_params.json

@@ -6,16 +6,16 @@
         1000.0
     ],
     "a_max_1": [
-        7.96
+        0.7
     ],
     "v_max_1": [
-        0.499
+        10.0
     ],
     "a_max_2": [
-        35.81
+        100.0
     ],
     "v_max_2": [
-        0.75
+        0.2
     ],
     "mass_1": [
         270.0
@@ -48,7 +48,7 @@
         0.0
     ],
     "k_prop": [
-        0.005
+        0.0075
     ],
     "time_capture": [
         1000.0

src/OptAlgorithm/OptAlgorithm.py

@@ -16,6 +16,12 @@ class OptAlgorithm(AutoConfigClass):
 
         calc = OptTimeCalculator(operator_config, system_config)
 
+        if self.distance_l_1 < 0:
+            raise Exception("Ошибка - заготовка вышла за пределы маскимального раскрытия электрода FE")
+
+        if self.distance_l_2 < 0:
+            raise Exception("Ошибка - заготовка вышла за пределы маскимального раскрытия электрода ME")
+
         self.Ts = calc.T(self.distance_h_start_1,
                          self.distance_h_start_2,
                          self.distance_s_1,
@@ -221,7 +227,8 @@ class OptAlgorithm(AutoConfigClass):
     def FMovement(self, t: float):
         x1 = self.X1Movement(t)
         x2 = self.X2Movement(t)
-        F = self.k_hardness_1 * max(0, (x1 - self.x1_contact))
+        #its not true, but theres no need to track collisions at this point
+        F = 0#self.k_hardness_1 * max(0, (x1 - self.x1_contact))
         return F
     
     def X1Movement(self, t: float):

src/OptAlgorithm/OptTimeCalculator.py

@@ -1,4 +1,5 @@
-from numpy import sqrt, arcsin, arccos, cos, sin
+import numpy as np
+from numpy import sqrt, arcsin, arccos, cos, sin, array
 
 from OptAlgorithm.AutoConfigClass import AutoConfigClass
 from OptAlgorithm.ConstantCalculator import ConstantCalculator
@@ -21,8 +22,8 @@ class OptTimeCalculator(AutoConfigClass):
         v0q = min(sqrt(2 * self.a_max_1 * h1), self.v_max_1)
         v0 = min(v0q, sqrt(1 / (self.k_hardness_1 * self.mass_1)) * self.Ftogrow)
         t1 = v0 / self.a_max_1
-        hleft = (h1 - (self.a_max_1 * t1 * t1 / 2))
+        hleft = max(0, h1 - (self.a_max_1 * t1 * t1 / 2))
-        t3 = min((v0q - v0)/ self.a_max_1, sqrt(self.a_max_1 * hleft))
+        t3 = min(max(0, v0q - v0)/ self.a_max_1, sqrt(self.a_max_1 * hleft))
         v1 = (t1 + t3) * self.a_max_1
         hleft2 = h1 - (self.a_max_1 * (t1+t3) * (t1+t3) / 2) - v1 * t3 + t3**2 * self.a_max_1 / 2
         t2t = max(0, hleft2 / v1)
@@ -86,7 +87,7 @@ class OptTimeCalculator(AutoConfigClass):
             v1 = topen_1_acc * self.a_max_1
             if s1 > topen_1_speed * v1:
                 s1 -= topen_1_speed * v1
-                topen_1_mark = 2 * topen_1_acc + topen_1_speed - sqrt(topen_1_acc ** 2 - 2 * s1 / self.a_max_1)
+                topen_1_mark = 2 * topen_1_acc + topen_1_speed - sqrt(max(0,topen_1_acc ** 2 - 2 * s1 / self.a_max_1))
             else:
                 topen_1_mark = topen_1_acc + s1 / v1
 
@@ -96,7 +97,7 @@ class OptTimeCalculator(AutoConfigClass):
             v2 = topen_2_acc * self.a_max_2
             if s2 > topen_2_speed * v2:
                 s2 -= topen_2_speed * v2
-                topen_2_mark = 2 * topen_2_acc + topen_2_speed - sqrt(topen_2_acc ** 2 - 2 * s2 / self.a_max_2)
+                topen_2_mark = 2 * topen_2_acc + topen_2_speed - sqrt(max(0,topen_2_acc ** 2 - 2 * s2 / self.a_max_2))
             else:
                 topen_2_mark = topen_2_acc + s2 / v2
 
@@ -108,7 +109,7 @@ class OptTimeCalculator(AutoConfigClass):
         v0 = self.allTimes["tclose_1_acc"] * self.a_max_1
         vF0 = v0 * self.k_hardness_1
 
-        vFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1 / (self.mass_1)) * self.Ftogrow)
+        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) * (
@@ -159,56 +160,78 @@ class OptTimeCalculator(AutoConfigClass):
             Tfull = self.time_robot_movement
 
             L = self.__dict__["distance_l_" + str(i)]
-            maxL = contact[i - 1] - L - x0
+            maxL = L + x0
             self.Tmovementi(i, x, Tfull, v0, maxL)
         return pos0s, v0s
 
     def Tmovementi(self, i, Sfull, Tfull, v0, maxL) -> None:
         v0 = abs(v0)
         vmax = self.__dict__["v_max_" + str(i)]
+
+        if vmax < v0 - self.check_eps:
+            raise Exception("Ошибка - скорость в начале перемещения больше макимальной - проверьте скорость")
         a = self.__dict__["a_max_" + str(i)]
-        t3 = (Tfull + v0 / a) / 2
+        Sfull = -Sfull
 
-        sqrtval = a ** 2 * (
+        time_eps = 1e-8
-                    a ** 2 * (Tfull + 2 * t3) ** 2 - 8 * a * Sfull + 2 * a * v0 * (Tfull + 2 * t3) - 3 * v0 ** 2)
+        T = 0
-        if sqrtval < 0:
+        if Sfull > 0:
-            raise Exception("""Невозможно с S_{i} добраться но H*_{i} за указанное время, 
+            S1_min = max(Sfull,v0*v0 / (2*a)) + 1e-7
-                            проверьте distance_s_{i}, distance_h_end{i}, time_command, time_robot_movement""")
+        else:
-        t1max = ((Tfull + 2 * t3) + v0 / a) / (2) - sqrt(sqrtval) * sqrt(2) / (4 * a ** 2)
+            S1_min = v0*v0 / (2*a) + 1e-7
-        t1 = min(t1max, (vmax - abs(v0)) / a)
+        S1_max = maxL
-        t1Llimit = -v0 / a + sqrt(v0 ** 2 / (a ** 2) + (abs(maxL) - v0 * v0 / a) / a)
+        if S1_min > S1_max:
-        t1 = max(0, min(t1, t1Llimit))
+            raise Exception("Ошибка - невозможно перемещение робота из-за слишком большого оффсета следующей заготовки - не хватает максимального раскрытия")
-        t31 = v0 / a + t1        
+        # calc for S1
-        v1 = v0 + a * t1
+        def calc_Tmovement_for_S1(S1):
-        
+            S2 = -Sfull + S1
-        #try to push t2 to limit S3_2-5
+            if S2 < 0:
-        maxiter = 10
+                raise Exception("Ошибка - прохождение второй части перемещения - отрицательно, неизвестное поведение")
-        t2 = 0
-        for j in range(maxiter):
-            S1 = v0 * t1 + a * t1 * t1 / 2 + v1 * t31 - a * t31 * t31 / 2 + t2 * v1
-            S2max = Sfull + S1
-            t5max = sqrt(S2max / a)
-            t5 = min(t5max, (vmax) / a)
-            t32 = t5
-            t3 = t31 + t32
 
+            t1_theory = -v0 / a + sqrt(v0 * v0 / (2 * a * a) + S1 / a)
+            t1 = min(t1_theory, max(0, vmax - v0) / a)
+            v2 = v0 + t1 * a
+            t31 = v2 / a
+            S1_fact = v0 * t1 + a * t1 ** 2 / 2 + v2 * t31 - a * t31 ** 2 / 2
+            t2 = max(0, (S1 - S1_fact) / v2)
+            T1 = t1 + t31 + t2
 
-            v1 = abs(v0 + t1 * a)
+            t32_theory = sqrt(S2 / a)
-            v3 = abs(v0 + t1 * a - t3 * a)
+            t32 = min(t32_theory, (vmax) / a)
-            timeleft = Tfull - t1 - t5 - t3
+            v4 = t32 * a
-            sq = -v0 * t1 - a * t1 ** 2 / 2 - v1 * t3 + a * t3 ** 2 / 2 + v3 * t5 - a * t5 ** 2 / 2
+            t5 = t32
-            Sleft = Sfull - sq
+            S2_fact = a * t32 ** 2 / 2 + v4 * t5 - a * t5 ** 2 / 2
+            t4 = max(0, (S2 - S2_fact) / v4)
+            T2 = t32 + t4 + t5
+            T = T1 + T2
+            return T, (t1, t2, t31, t32, t4, t5)
 
-            t2max = (timeleft - Sleft / v3) / (1 + v1 / v3)
+        T_min, _ = calc_Tmovement_for_S1(S1_min)
-            Smovement = -v0 * t1 - a / 2 * t1 ** 2 - v1 * t31 + a / 2 * t31 ** 2
+        if T_min > Tfull:
-            if v1 == 0:
+            raise Exception(f"""Ошибка - время перемещения слишком мало, чтобы хотя бы закончить раскрытие, проверьте скорость, ускорение, время перемещения робота """)
-                t2 = 0
+        T_max, _ = calc_Tmovement_for_S1(S1_max)
-            else:
+        if T_max < Tfull:
-                t2 = max(0, min(t2max, (abs(maxL) - abs(Smovement)) / v1))
+            S1 = S1_max
-            t4 = max(0, Sleft / v3 + v1 / v3 * t2)
+        else:
-
+            maxiter = 20
-            tstay = max(0, Tfull - t1 - t2 - t3 - t4 - t5)
+            cur_iter = 0
+            while abs(T - Tfull) > time_eps and S1_max > S1_min and cur_iter < maxiter:
+                S1_cur = (S1_min + S1_max)/2
+                T, _ = calc_Tmovement_for_S1(S1_cur)
+                if T > Tfull:
+                    S1_max = S1_cur
+                else:
+                    S1_min = S1_cur
+                cur_iter += 1
+            S1 = S1_min
+        T, tarray = calc_Tmovement_for_S1(S1)
+        tstay = max(0, Tfull - T)
 
+        t1, t2, t31, t32, t4, t5 = tarray
+        for j, t in enumerate(tarray):
+            if t < 0:
+                raise Exception(f"""Ошибка - время перехода во время фазы {j + 1} {i}-го электрода отрицательно - переход невозможен с такими параметрами,
+                                    проверьте скорость, ускорение, смещение """)
         self.allTimes["tmovement_" + str(i) + "_acc"] = t1
         self.allTimes["tmovement_" + str(i) + "_speed"] = t2
         self.allTimes["tmovement_" + str(i) + "_slow"] = t31
@@ -226,7 +249,7 @@ class OptTimeCalculator(AutoConfigClass):
         v0 = min(v0q, sqrt(1 / (self.k_hardness_1 * self.mass_1)) * self.Ftogrow)
         t2 = T - sqrt(max(0, T ** 2 - 2 * s / self.a_max_1))
         if t2 * self.a_max_1 < v0:
-            #we should wait to end with max speed
+            #we should wait to end with max speed but dont do it
             t2 = v0 / self.a_max_1
             t3 = max(0, (s - self.a_max_1 * t2 ** 2 / 2) / (self.a_max_1 * t2))
             t1 = T - t2 - t3