From e1c38eeb2dfddfd1dedd4f6d765835ada794df89 Mon Sep 17 00:00:00 2001 From: ermolaev_p Date: Fri, 15 Nov 2024 14:34:31 +0300 Subject: [PATCH 1/5] =?UTF-8?q?fix(SF-483):=20=D0=98=D1=81=D0=BF=D1=80?= =?UTF-8?q?=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD=20=D0=BD=D0=B5=D0=BF=D1=80=D0=B0?= =?UTF-8?q?=D0=B2=D0=B8=D0=BB=D1=8C=D0=BD=D1=8B=D0=B9=20=D0=BF=D0=BE=D0=B4?= =?UTF-8?q?=D1=81=D1=87=D1=91=D1=82=20=D1=81=D0=BA=D0=BE=D1=80=D0=BE=D1=81?= =?UTF-8?q?=D1=82=D0=B8=20=D0=BD=D0=B0=20=D1=8D=D1=82=D0=B0=D0=BF=D0=B5=20?= =?UTF-8?q?=D1=80=D0=B0=D1=81=D0=BA=D1=80=D1=8B=D1=82=D0=B8=D1=8F,=20?= =?UTF-8?q?=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD=20=D0=B0=D1=81?= =?UTF-8?q?=D1=81=D0=B5=D1=80=D1=82=20=D0=BD=D0=B0=20=D0=B4=D0=BE=D1=81?= =?UTF-8?q?=D1=82=D0=B8=D0=B6=D0=B5=D0=BD=D0=B8=D0=B5=20=D0=BD=D0=BE=D0=BC?= =?UTF-8?q?=D0=B8=D0=BD=D0=B0=D0=BB=D1=8C=D0=BD=D0=BE=D0=B9=20=D1=82=D1=80?= =?UTF-8?q?=D0=B0=D0=B5=D0=BA=D1=82=D0=BE=D1=80=D0=B8=D0=B8=20=D0=B4=D0=BE?= =?UTF-8?q?=20=D1=84=D0=B0=D0=B7=D1=8B=20=D0=BF=D0=BE=D0=B4=D0=BF=D0=BE?= =?UTF-8?q?=D1=80=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/OptAlgorithm/OptTimeCalculator.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/OptAlgorithm/OptTimeCalculator.py b/src/OptAlgorithm/OptTimeCalculator.py index aa1d9ce..17ca585 100644 --- a/src/OptAlgorithm/OptTimeCalculator.py +++ b/src/OptAlgorithm/OptTimeCalculator.py @@ -51,12 +51,10 @@ class OptTimeCalculator(AutoConfigClass): t22 = max(0, (l2 - (self.a_max_2 * t21 * t21)) / self.v_max_2) T2 = t22 + 2 * t21 + offset - Topen = max(T1, T2) - - topen_1_acc, topen_1_speed = self.calcFirstOpen(Topen, 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(Topen - offset, l2) + 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 @@ -95,7 +93,6 @@ class OptTimeCalculator(AutoConfigClass): self.allTimes["topen_1_mark"] = topen_1_mark self.allTimes["topen_2_mark"] = topen_2_mark - self.allTimes["topen"] = Topen def Tgrow(self) -> None: @@ -109,7 +106,9 @@ class OptTimeCalculator(AutoConfigClass): 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) + Fstart_prop = self.Fstart_prop + assert Fmeet < Fstart_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) From 601f7912cfc4f4b739c69b882994c785e7173a0b Mon Sep 17 00:00:00 2001 From: ermolaev_p Date: Fri, 15 Nov 2024 15:57:31 +0300 Subject: [PATCH 2/5] =?UTF-8?q?fix(SF-483):=20=D0=94=D0=BE=D0=B1=D0=B0?= =?UTF-8?q?=D0=B2=D0=BB=D0=B5=D0=BD=D1=8B=20=D0=BE=D1=88=D0=B8=D0=B1=D0=BA?= =?UTF-8?q?=D0=B8=20=D0=BF=D0=BE=20=D0=BD=D0=B5=D1=80=D0=BF=D0=B0=D0=B2?= =?UTF-8?q?=D0=B8=D0=BB=D1=8C=D0=BD=D1=8B=D0=BC=20=D0=BD=D0=B0=D0=B1=D0=BE?= =?UTF-8?q?=D1=80=D0=B0=D0=BC=20=D0=BF=D0=B0=D1=80=D0=B0=D0=BC=D0=B5=D1=82?= =?UTF-8?q?=D1=80=D0=BE=D0=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/OptAlgorithm/ConstantCalculator.py | 7 +++-- src/OptAlgorithm/OptTimeCalculator.py | 41 ++++++++++++++++---------- 2 files changed, 29 insertions(+), 19 deletions(-) diff --git a/src/OptAlgorithm/ConstantCalculator.py b/src/OptAlgorithm/ConstantCalculator.py index 7146fa2..24c5714 100644 --- a/src/OptAlgorithm/ConstantCalculator.py +++ b/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 diff --git a/src/OptAlgorithm/OptTimeCalculator.py b/src/OptAlgorithm/OptTimeCalculator.py index 17ca585..5b103aa 100644 --- a/src/OptAlgorithm/OptTimeCalculator.py +++ b/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: - raise ValueError("S1 > L1 - недопустимый сценарий") - if s2 > l2: - raise ValueError("S2 > L2 - недопустимый сценарий") - + if s1 >= l1: + raise Exception("""S1 >= L1 - недопустимый сценарий, + проверьте dist_open_after_1, dist_close_end_1""") + 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 - assert tq > 0 - v0 = closeAlgo("V"+str(i), "Open", tq) + if tmark < 0: + raise Exception("""Отрицательное время этапа раскрытия, + проверьте 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 From bd65193bf0eab40f1294d566991edc286b99f746 Mon Sep 17 00:00:00 2001 From: ermolaev_p Date: Fri, 15 Nov 2024 16:38:34 +0300 Subject: [PATCH 3/5] =?UTF-8?q?fix(SF-483):=20=D0=98=D1=81=D0=BF=D1=80?= =?UTF-8?q?=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD=D0=B0=20=D0=BE=D1=88=D0=B8=D0=B1?= =?UTF-8?q?=D0=BA=D0=B0=20=D1=81=20=D1=80=D0=B0=D1=81=D1=87=D0=B5=D1=82?= =?UTF-8?q?=D0=BE=D0=BC=20=D1=82=D1=80=D0=B0=D0=B5=D0=BA=D1=82=D0=BE=D1=80?= =?UTF-8?q?=D0=B8=D0=B8=20=D0=B7=D0=B0=D0=BA=D1=80=D1=8B=D1=82=D0=B8=D1=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/OptAlgorithm/OptAlgorithm.py | 3 ++- src/OptAlgorithm/OptTimeCalculator.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/OptAlgorithm/OptAlgorithm.py b/src/OptAlgorithm/OptAlgorithm.py index c86e411..e0e8108 100644 --- a/src/OptAlgorithm/OptAlgorithm.py +++ b/src/OptAlgorithm/OptAlgorithm.py @@ -116,7 +116,8 @@ class OptAlgorithm(AutoConfigClass): dF0 = self.a_max_1 * self.Ts["tclose_1_acc"] * self.k_hardness_1 dFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow) - self.Fmeet = 1/ self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - dFmax**2) + eps = 1e1 + self.Fmeet = 1/ self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - dFmax**2 + eps) tspeed = self.Ts["tspeed"] Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1/self.freq * dF0 * sin(self.freq * tspeed) if t < self.Ts["tspeed"]: diff --git a/src/OptAlgorithm/OptTimeCalculator.py b/src/OptAlgorithm/OptTimeCalculator.py index 5b103aa..d5b93af 100644 --- a/src/OptAlgorithm/OptTimeCalculator.py +++ b/src/OptAlgorithm/OptTimeCalculator.py @@ -173,7 +173,7 @@ class OptTimeCalculator(AutoConfigClass): t5max = (Tfull - v0/a)/2 - t1 v1 = v0 + a * t1 S1 = v0*t1 + a*t1*t1/2 + v1*t31 - a*t31*t31/2 - S2max = abs(Sfull) + abs(S1) + S2max = Sfull + S1 t5 = min(t5max, (vmax)/a, sqrt(S2max / a)) t3 = abs(v0)/a + t1 + t5 t32 = t5 From 71c82855833552f136960905a48fa36aed26b260 Mon Sep 17 00:00:00 2001 From: ermolaev_p Date: Mon, 2 Dec 2024 12:46:10 +0300 Subject: [PATCH 4/5] =?UTF-8?q?feat:=20=D0=94=D0=BE=D0=B1=D0=B0=D0=B2?= =?UTF-8?q?=D0=BB=D0=B5=D0=BD=20=D1=81=D0=B8=D0=B3=D0=BD=D0=B0=D0=BB=20Onc?= =?UTF-8?q?omming,=20=D0=B1=D0=B5=D0=B7=20=D0=BE=D0=B1=D1=80=D0=B0=D0=B1?= =?UTF-8?q?=D0=BE=D1=82=D0=BA=D0=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- params/system_params.json | 17 +++++++++-------- src/utils/diagram_parser.py | 5 ++++- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/params/system_params.json b/params/system_params.json index 017e7fb..5749167 100644 --- a/params/system_params.json +++ b/params/system_params.json @@ -1,11 +1,11 @@ { "a_max_1": 5.41, - "v_max_1": 0.108, + "v_max_1": 0.278, "a_max_2": 35.81, - "v_max_2": 0.678, - "mass_1": 257, + "v_max_2": 0.7, + "mass_1": 270, "mass_2": 1, - "k_hardness_1": 1759291, + "k_hardness_1": 2148570, "k_hardness_2": 0, "torque_max_1": 20, "torque_max_2": 0, @@ -13,11 +13,12 @@ "transmission_ratio_2": 1, "position_start_1": 0.08, "position_start_2": 0.08, - "k_prop": 0.05, - "time_capture": 100000, + "k_prop": 0.075, + "time_capture": 10, "UML_time_scaler": 1000, "Closure_signal": "Closing", "Squeeze_signal": "Squeeze", "Welding_signal": "Welding", - "Release_signal": "Relief" -} + "Release_signal": "Relief", + "Oncomming_signal": "Oncomming" +} \ No newline at end of file diff --git a/src/utils/diagram_parser.py b/src/utils/diagram_parser.py index 5c7451d..c72559a 100644 --- a/src/utils/diagram_parser.py +++ b/src/utils/diagram_parser.py @@ -7,7 +7,8 @@ class DiagramParser: self.signals = [system_config["Closure_signal"], system_config["Squeeze_signal"], system_config["Welding_signal"], - system_config["Release_signal"]] + system_config["Release_signal"], + system_config["Oncomming_signal"]] self.boolDict = {} self.floatDict = {} @@ -22,6 +23,7 @@ class DiagramParser: for signalName in self.data.columns: if type (self.data[signalName].iloc[0]) == np.bool: + print("!", signalName) self.boolDict[signalName] = self._getBoolChanges(signalName) for signalName in self.data.columns: @@ -33,6 +35,7 @@ class DiagramParser: elif key == self.signals[1]: name = "compression" elif key == self.signals[2]: name = "welding" elif key == self.signals[3]: name = "opening" + elif key == self.signals[4]: name = "oncomming" self.timingsDict[name] = [] len_items = len(items) From 918663fa4490c5291a4a7c5e047dea16d18a5b76 Mon Sep 17 00:00:00 2001 From: ermolaev_p Date: Mon, 2 Dec 2024 12:47:47 +0300 Subject: [PATCH 5/5] =?UTF-8?q?chore:=20=D0=A3=D0=B1=D1=80=D0=B0=D0=BD=20?= =?UTF-8?q?=D0=BE=D1=82=D0=BB=D0=B0=D0=B4=D0=BE=D1=87=D0=BD=D1=8B=D0=B9=20?= =?UTF-8?q?=D0=B2=D1=8B=D0=B2=D0=BE=D0=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/utils/diagram_parser.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/utils/diagram_parser.py b/src/utils/diagram_parser.py index c72559a..aceb9ce 100644 --- a/src/utils/diagram_parser.py +++ b/src/utils/diagram_parser.py @@ -23,7 +23,6 @@ class DiagramParser: for signalName in self.data.columns: if type (self.data[signalName].iloc[0]) == np.bool: - print("!", signalName) self.boolDict[signalName] = self._getBoolChanges(signalName) for signalName in self.data.columns: