diff --git a/params/operator_params.json b/params/operator_params.json index cc36187..37687f7 100644 --- a/params/operator_params.json +++ b/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, - 5000.0, + 2000.0, 5000.0, 5000.0, 5000.0, diff --git a/params/system_params.json b/params/system_params.json index 332939d..f69a783 100644 --- a/params/system_params.json +++ b/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 diff --git a/src/OptAlgorithm/OptAlgorithm.py b/src/OptAlgorithm/OptAlgorithm.py index 747d935..0491f9d 100644 --- a/src/OptAlgorithm/OptAlgorithm.py +++ b/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): diff --git a/src/OptAlgorithm/OptTimeCalculator.py b/src/OptAlgorithm/OptTimeCalculator.py index fa28fa7..b30b549 100644 --- a/src/OptAlgorithm/OptTimeCalculator.py +++ b/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)) - t3 = min((v0q - v0)/ self.a_max_1, sqrt(self.a_max_1 * hleft)) + hleft = max(0, h1 - (self.a_max_1 * t1 * t1 / 2)) + 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 * ( - 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""") - t1max = ((Tfull + 2 * t3) + v0 / a) / (2) - sqrt(sqrtval) * sqrt(2) / (4 * a ** 2) - t1 = min(t1max, (vmax - abs(v0)) / a) - t1Llimit = -v0 / a + sqrt(v0 ** 2 / (a ** 2) + (abs(maxL) - v0 * v0 / a) / a) - t1 = max(0, min(t1, t1Llimit)) - t31 = v0 / a + t1 - v1 = v0 + a * t1 - - #try to push t2 to limit S3_2-5 - maxiter = 10 - 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 + time_eps = 1e-8 + T = 0 + if Sfull > 0: + S1_min = max(Sfull,v0*v0 / (2*a)) + 1e-7 + else: + S1_min = v0*v0 / (2*a) + 1e-7 + S1_max = maxL + if S1_min > S1_max: + raise Exception("Ошибка - невозможно перемещение робота из-за слишком большого оффсета следующей заготовки - не хватает максимального раскрытия") + # calc for S1 + def calc_Tmovement_for_S1(S1): + S2 = -Sfull + S1 + if S2 < 0: + raise Exception("Ошибка - прохождение второй части перемещения - отрицательно, неизвестное поведение") + 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) - v3 = abs(v0 + t1 * a - t3 * a) - timeleft = Tfull - t1 - t5 - t3 - sq = -v0 * t1 - a * t1 ** 2 / 2 - v1 * t3 + a * t3 ** 2 / 2 + v3 * t5 - a * t5 ** 2 / 2 - Sleft = Sfull - sq + t32_theory = sqrt(S2 / a) + t32 = min(t32_theory, (vmax) / a) + v4 = t32 * a + t5 = t32 + 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) - Smovement = -v0 * t1 - a / 2 * t1 ** 2 - v1 * t31 + a / 2 * t31 ** 2 - if v1 == 0: - t2 = 0 - else: - t2 = max(0, min(t2max, (abs(maxL) - abs(Smovement)) / v1)) - t4 = max(0, Sleft / v3 + v1 / v3 * t2) - - tstay = max(0, Tfull - t1 - t2 - t3 - t4 - t5) + T_min, _ = calc_Tmovement_for_S1(S1_min) + if T_min > Tfull: + raise Exception(f"""Ошибка - время перемещения слишком мало, чтобы хотя бы закончить раскрытие, проверьте скорость, ускорение, время перемещения робота """) + T_max, _ = calc_Tmovement_for_S1(S1_max) + if T_max < Tfull: + S1 = S1_max + else: + maxiter = 20 + 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