fix(SF-483): Добавлены ошибки по нерпавильным наборам параметров

src/OptAlgorithm/ConstantCalculator.py

@@ -8,13 +8,14 @@ class ConstantCalculator(AutoConfigClass):
         
         super().__init__(ConstantCalculator.params_list, operator_config, system_config)
         
+        for param, value in self.__dict__.items():
+            if value is int and value < 0:
+                raise Exception("""Недопустимое значение параметра {param} < 0""")
+        
     def calc(self):
         constants = {}
         #self.smin1t = self.smin1 - self.dblock / 2
         #self.smin2t = self.smin2 - self.dblock / 2
-        #self.awork = self.umax / (self.l * self.m)
-        #self.fl = self.Fd * (1-self.kturn)
-        #self.flon = self.Fd * self.kturn
         constants["Fprop"] = self.k_prop * self.force_target
         constants["freq"] = sqrt(self.k_hardness_1 / self.mass_1)
         constants["eff_control"] = self.torque_max_1 / self.transmission_ratio_1

src/OptAlgorithm/OptTimeCalculator.py

@@ -65,11 +65,13 @@ class OptTimeCalculator(AutoConfigClass):
         self.allTimes["topen_2_acc"] = topen_2_acc
         self.allTimes["topen_2_speed"] = topen_2_speed
         
-        if s1 > l1:
+        if s1 >= l1:
-            raise ValueError("S1 > L1 - недопустимый сценарий")
+            raise Exception("""S1 >= L1 - недопустимый сценарий,
-        if s2 > l2:
+                            проверьте dist_open_after_1, dist_close_end_1""")
-            raise ValueError("S2 > L2 - недопустимый сценарий")
+        if s2 >= l2:
-        
+            raise Exception("""S2 >= L2 - недопустимый сценарий,
+                            проверьте dist_open_after_2, dist_close_end_2""")
+            
         s1 += Fs1
         topen_1_mark = sqrt(2 * s1 / self.a_max_1)
         if topen_1_mark > topen_1_acc:
@@ -100,15 +102,19 @@ class OptTimeCalculator(AutoConfigClass):
         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)
-        l = sqrt(self.eff_control ** 2 + self.mass_1/self.k_hardness_1 * vF0**2)
 
         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))
         Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1/self.freq * vF0 * sin(self.freq * tspeed)
-        Fmeet =  1/self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - vFmax**2)
+        eps = 1e1
-
+        if self.freq**2 * self.Ftogrow**2 - vFmax**2 < -eps:
+            raise Exception("""Номинальная траектория набора усилия не может быть достигнута, максимальная скорость превысила скорость траектории
+                            , проверьте параметры k_hardness_1, mass_1, k_prop""")
+        Fmeet =  1/self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - vFmax**2 + eps)
         Fstart_prop = self.Fstart_prop
-        assert Fmeet < Fstart_prop
+        if Fmeet > Fstart_prop:
+            raise Exception("""Номинальная траектория набора усилия была достигнута на фазе подпора
+                            , проверьте параметры v_max_1, k_prop""")
         tmeet = (Fmeet - Fspeed)/vFmax
         tend = self.tGrowNominal(Fstart_prop) - self.tGrowNominal(Fmeet)
         vp = 1/sqrt(self.k_hardness_1 * self.mass_1) * sqrt(self.Ftogrow**2 - self.Fstart_prop**2)
@@ -132,14 +138,15 @@ class OptTimeCalculator(AutoConfigClass):
         v0s = []
         pos0s = []
         for i in range(1,3): 
-            tq = tmark
+            if tmark < 0:
-            assert tq > 0
+                raise Exception("""Отрицательное время этапа раскрытия, 
-            v0 = closeAlgo("V"+str(i), "Open", tq)
+                                проверьте dist_open_after_{1,2}, time_command""")
+            v0 = closeAlgo("V"+str(i), "Open", tmark)
             v0s.append(v0)
-            x0 = closeAlgo("X"+str(i), "Open", tq)
+            x0 = closeAlgo("X"+str(i), "Open", tmark)
             x1 = contact[i-1] - self.__dict__["dist_close_end_"+str(i)]
             x = x1 - x0
-            pos0s.append(closeAlgo("X"+str(i), "Open", tq))
+            pos0s.append(closeAlgo("X"+str(i), "Open", tmark))
             Tfull = self.time_robot_movement
             
             
@@ -155,7 +162,9 @@ class OptTimeCalculator(AutoConfigClass):
         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)
-        assert sqrtval >= 0
+        if sqrtval < 0:
+            raise Exception("""Невозможно с S_{i} добраться но H*_{i} за указанное время, 
+                            проверьте dist_open_after_{i}, dist_close_end_{i}, time_command, time_robot_movement""")
         t1max = ((Tfull+2*t3) + v0/a)/(2) - sqrt(sqrtval) * sqrt(2)/(4*a**2)
         t1 = min(t1max, (vmax- abs(v0))/a)
         t1 = max(0, min(t1, -v0/a + sqrt(v0**2 / (a**2) + (abs(maxL)-v0*v0/a)/a)))
@@ -180,7 +189,7 @@ class OptTimeCalculator(AutoConfigClass):
         t2 = max(0, min(t2max, (abs(maxL) - abs(Smovement))/v1))
         t4 = max(0, Sleft/v3 + v1/v3 * t2)        
         
-        tstay = Tfull - t1 - t2 - t3 - t4 - t5      
+        tstay = max(0, Tfull - t1 - t2 - t3 - t4 - t5)      
         
         self.allTimes["tmovement_"+str(i)+"_acc"] = t1
         self.allTimes["tmovement_"+str(i)+"_speed"] = t2