From ccfb6b9fcb566cb7badc3d3f2444348d2b944494 Mon Sep 17 00:00:00 2001 From: ermolaev_p Date: Mon, 23 Dec 2024 16:35:39 +0300 Subject: [PATCH] =?UTF-8?q?fix:=20=D0=98=D1=81=D0=BF=D1=80=D0=B0=D0=B2?= =?UTF-8?q?=D0=BB=D0=B5=D0=BD=D0=BE=20=D1=81=D0=BC=D1=8B=D0=BA=D0=B0=D0=BD?= =?UTF-8?q?=D0=B8=D0=B5=20=D0=BD=D0=B0=20=D0=B8=D0=B4=D0=B5=D0=B0=D0=BB?= =?UTF-8?q?=D1=8C=D0=BD=D0=BE=D0=B9=20=D1=82=D1=80=D0=B0=D0=B5=D0=BA=D1=82?= =?UTF-8?q?=D0=BE=D1=80=D0=B8=D0=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/OptAlgorithm/OptTimeCalculator.py | 261 +++++++++++++------------- src/testAlgo.py | 43 +++++ 2 files changed, 177 insertions(+), 127 deletions(-) create mode 100644 src/testAlgo.py diff --git a/src/OptAlgorithm/OptTimeCalculator.py b/src/OptAlgorithm/OptTimeCalculator.py index e893334..2630f4f 100644 --- a/src/OptAlgorithm/OptTimeCalculator.py +++ b/src/OptAlgorithm/OptTimeCalculator.py @@ -3,75 +3,76 @@ from numpy import sqrt, arcsin, arccos, cos, sin from OptAlgorithm.AutoConfigClass import AutoConfigClass from OptAlgorithm.ConstantCalculator import ConstantCalculator + class OptTimeCalculator(AutoConfigClass): - params_list = [] - def __init__(self, operator_config : dict, system_config : dict): - + + 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 = {} - - def tGrowNominal(self, F : float) -> float: - return arcsin(F/(self.Ftogrow)) * sqrt(self.mass_1/self.k_hardness_1) + self.check_eps = 1e-7 + + def tGrowNominal(self, F: float) -> float: + return arcsin(F / (self.Ftogrow)) * sqrt(self.mass_1 / self.k_hardness_1) def Tclose(self, h1: float, h2: float) -> None: 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) + v0 = min(v0q, sqrt(1 / (self.k_hardness_1 * self.mass_1)) * self.Ftogrow) t1 = v0 / self.a_max_1 - t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 /2)) / v0) + t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 / 2)) / v0) T1 = t1 + t2t - - t21 = sqrt(h2/self.a_max_2) - t21 = min(self.v_max_2/self.a_max_2, t21) + + 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 - + Tclose = max(T1, T2) - + tclose_1_acc, tclose_1_speed = self.calcFirstClose(Tclose, h1) tclose_2_acc, tclose_2_speed = self.calcSecondClose(Tclose, h2) - + self.allTimes["tclose_1_acc"] = tclose_1_acc self.allTimes["tclose_1_speed"] = tclose_1_speed - + self.allTimes["tclose_2_acc"] = tclose_2_acc self.allTimes["tclose_2_speed"] = tclose_2_speed self.allTimes["tclose"] = Tclose - - def Topen(self, s1 : float, s2 : float, l1 : float, l2 : float, Fs1 : float, Fs2 : float = 0) -> None: - t11 = sqrt((l1 + Fs1)/self.a_max_1) - t11 = min(self.v_max_1/self.a_max_1, t11) - t12 = max(0, ((l1+Fs1) - (self.a_max_1 * t11 * t11)) / self.v_max_1) + + def Topen(self, s1: float, s2: float, l1: float, l2: float, Fs1: float, Fs2: float = 0) -> None: + t11 = sqrt((l1 + Fs1) / self.a_max_1) + t11 = min(self.v_max_1 / self.a_max_1, t11) + t12 = max(0, ((l1 + Fs1) - (self.a_max_1 * t11 * t11)) / self.v_max_1) T1 = t12 + 2 * t11 offset = self.calcSecondOpenOffset(t11, t12, Fs1) - - t21 = sqrt(l2/self.a_max_2) - t21 = min(self.v_max_2/self.a_max_2, t21) + + t21 = sqrt(l2 / self.a_max_2) + t21 = min(self.v_max_2 / self.a_max_2, t21) t22 = max(0, (l2 - (self.a_max_2 * t21 * t21)) / self.v_max_2) T2 = t22 + 2 * t21 + offset - - topen_1_acc, topen_1_speed = self.calcFirstOpen(T1, l1+Fs1) + + 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 - self.allTimes["topen_2_offset"] = offset - + + self.allTimes["topen_1_acc"] = topen_1_acc + self.allTimes["topen_2_offset"] = offset + self.allTimes["topen_1_acc"] = topen_1_acc self.allTimes["topen_1_speed"] = topen_1_speed - + self.allTimes["topen_2_acc"] = topen_2_acc self.allTimes["topen_2_speed"] = topen_2_speed - + if s1 >= l1: raise Exception("""S1 >= L1 - недопустимый сценарий, проверьте distance_s_1, distance_h_end1""") if s2 >= l2: raise Exception("""S2 >= L2 - недопустимый сценарий, проверьте distance_s_2, distance_h_end2""") - + s1 += Fs1 topen_1_mark = sqrt(2 * s1 / self.a_max_1) if topen_1_mark > topen_1_acc: @@ -79,167 +80,173 @@ 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(topen_1_acc ** 2 - 2 * s1 / self.a_max_1) else: topen_1_mark = topen_1_acc + s1 / v1 - + topen_2_mark = sqrt(2 * s2 / self.a_max_2) if topen_2_mark > topen_2_acc: s2 -= topen_2_acc ** 2 * self.a_max_2 / 2 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(topen_2_acc ** 2 - 2 * s2 / self.a_max_2) else: topen_2_mark = topen_2_acc + s2 / v2 self.allTimes["topen_1_mark"] = topen_1_mark self.allTimes["topen_2_mark"] = topen_2_mark - - def Tgrow(self) -> None: - - 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) - 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) + def Tgrow(self) -> None: + + 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) + + 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) eps = 1e1 - if self.freq**2 * self.Ftogrow**2 - vFmax**2 < -eps: + 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) + Fmeet = 1 / self.freq * sqrt(self.freq ** 2 * self.Ftogrow ** 2 - vFmax ** 2 + eps) Fstart_prop = self.Fstart_prop if Fmeet > Fstart_prop: raise Exception("""Номинальная траектория набора усилия была достигнута на фазе подпора , проверьте параметры v_max_1, k_prop""") - tmeet = (Fmeet - Fspeed)/vFmax + 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) + vp = 1 / sqrt(self.k_hardness_1 * self.mass_1) * sqrt(self.Ftogrow ** 2 - self.Fstart_prop ** 2) ap = Fstart_prop / self.mass_1 - tprop = 2*vp / ap - + tprop = 2 * vp / ap + self.allTimes["tspeed"] = tspeed self.allTimes["tmeet"] = tmeet self.allTimes["tend"] = tend self.allTimes["tprop"] = tprop self.allTimes["tgrow"] = tspeed + tmeet + tend + tprop - - def T(self, h1 : float, h2 : float, s1 : float, s2 : float, l1 : float, l2 : float) -> dict: + + def T(self, h1: float, h2: float, s1: float, s2: float, l1: float, l2: float) -> dict: self.Tclose(h1, h2) self.Tgrow() 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 = [] - for i in range(1,3): + for i in range(1, 3): if tmark < 0: raise Exception("""Отрицательное время этапа раскрытия, проверьте distance_s_{1,2}, time_command""") - v0 = closeAlgo("V"+str(i), "Open", tmark) + v0 = closeAlgo("V" + str(i), "Open", tmark) v0s.append(v0) - x0 = closeAlgo("X"+str(i), "Open", tmark) - x1 = contact[i-1] - self.__dict__["distance_h_end"+str(i)] + x0 = closeAlgo("X" + str(i), "Open", tmark) + x1 = contact[i - 1] - self.__dict__["distance_h_end" + str(i)] x = x1 - x0 - pos0s.append(closeAlgo("X"+str(i), "Open", tmark)) + 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 + + L = self.__dict__["distance_l_" + str(i)] + maxL = contact[i - 1] - 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)] - 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) + vmax = self.__dict__["v_max_" + str(i)] + 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) 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) - t1 = max(0, min(t1, -v0/a + sqrt(v0**2 / (a**2) + (abs(maxL)-v0*v0/a)/a))) - - t31 = v0/a + t1 - t5max = (Tfull - v0/a)/2 - t1 + 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))) + + t31 = v0 / a + t1 + t5max = (Tfull - v0 / a) / 2 - t1 v1 = v0 + a * t1 - S1 = v0*t1 + a*t1*t1/2 + v1*t31 - a*t31*t31/2 + S1 = v0 * t1 + a * t1 * t1 / 2 + v1 * t31 - a * t31 * t31 / 2 S2max = Sfull + S1 - t5 = min(t5max, (vmax)/a, sqrt(S2max / a)) - t3 = abs(v0)/a + t1 + t5 + t5 = min(t5max, (vmax) / a, sqrt(S2max / a)) + t3 = abs(v0) / a + t1 + t5 t32 = t5 - - v1 = abs(v0+t1*a) - v3 = abs(v0 + t1*a - t3*a) + + 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 + sq = -v0 * t1 - a * t1 ** 2 / 2 - v1 * t3 + a * t3 ** 2 / 2 + v3 * t5 - a * t5 ** 2 / 2 Sleft = Sfull - sq - - t2max = (timeleft - Sleft/v3) / (1 + v1/v3) - Smovement = -v0 * t1 - a/2 * t1**2 - v1 * t31 + a/2*t31**2 - 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) - - self.allTimes["tmovement_"+str(i)+"_acc"] = t1 - self.allTimes["tmovement_"+str(i)+"_speed"] = t2 - self.allTimes["tmovement_"+str(i)+"_slow"] = t31 - self.allTimes["tmovement_"+str(i)+"_stay"] = tstay - self.allTimes["tmovement_"+str(i)] = t1 + t2 + t31 + tstay - self.allTimes["tpreclose_"+str(i)+"_slow"] = t32 - self.allTimes["tpreclose_"+str(i)+"_speed"] = t4 - self.allTimes["tpreclose_"+str(i)+"_acc"] = t5 - self.allTimes["tpreclose_"+str(i)] = t32 + t4 + t5 + + t2max = (timeleft - Sleft / v3) / (1 + v1 / v3) + Smovement = -v0 * t1 - a / 2 * t1 ** 2 - v1 * t31 + a / 2 * t31 ** 2 + 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) + + self.allTimes["tmovement_" + str(i) + "_acc"] = t1 + self.allTimes["tmovement_" + str(i) + "_speed"] = t2 + self.allTimes["tmovement_" + str(i) + "_slow"] = t31 + self.allTimes["tmovement_" + str(i) + "_stay"] = tstay + self.allTimes["tmovement_" + str(i)] = t1 + t2 + t31 + tstay + self.allTimes["tpreclose_" + str(i) + "_slow"] = t32 + self.allTimes["tpreclose_" + str(i) + "_speed"] = t4 + self.allTimes["tpreclose_" + str(i) + "_acc"] = t5 + self.allTimes["tpreclose_" + str(i)] = t32 + t4 + t5 T = Tfull self.allTimes["tmovement"] = T - - def calcFirstClose(self, T : float, s : float) -> tuple[float, float]: + + def calcFirstClose(self, T: float, s: float) -> tuple[float, float]: 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) - t2 = max(0, (s - self.a_max_1*t1**2/2) / (self.a_max_1*t1)) + 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)) + 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) - t2 = max(0, (s - self.a_max_1*t1**2/2) / (self.a_max_1*t1)) + + 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 + 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) - t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1)) + + 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 + if t1 > self.v_max_2 / self.a_max_2 + self.check_eps: + 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) - t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1)) + + 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 + if t1 > self.v_max_2 / self.a_max_2 + self.check_eps: + 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: + + def calcSecondOpenOffset(self, t1: float, t2: float, sq: float) -> float: s = sq * 1 offset = sqrt(2 * s / self.a_max_1) - + if offset > t1: s -= t1 ** 2 * self.a_max_1 / 2 v1 = t1 * self.a_max_1 if s > t2 * v1: s -= t2 * v1 - - offset = 2*t1 + t2 - sqrt(t1**2 - 2*s / self.a_max_1) + + offset = 2 * t1 + t2 - sqrt(t1 ** 2 - 2 * s / self.a_max_1) else: offset = t1 + s / v1 return offset diff --git a/src/testAlgo.py b/src/testAlgo.py new file mode 100644 index 0000000..f0b05a7 --- /dev/null +++ b/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() \ No newline at end of file