From 16416d60110f0bbb035491fd14314ab388a99adb Mon Sep 17 00:00:00 2001 From: ermolaev_p Date: Mon, 16 Dec 2024 14:02:42 +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=20=D0=BD=D0=B5=D0=BA=D0=BE=D1=80=D1=80=D0=B5?= =?UTF-8?q?=D0=BA=D1=82=D0=BD=D1=8B=D0=B9=20=D0=BF=D0=BE=D0=B4=D1=81=D1=87?= =?UTF-8?q?=D0=B5=D1=82=20=D1=82=D0=BE=D1=87=D0=BA=D0=B8=20=D1=81=D0=BC?= =?UTF-8?q?=D1=8B=D0=BA=D0=B0=D0=BD=D0=B8=D1=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- params/operator_params.json | 16 +++++------ params/system_params.json | 4 +-- src/OptAlgorithm/ConstantCalculator.py | 2 -- src/OptAlgorithm/OptAlgorithm.py | 37 +++++++++++++------------- src/OptAlgorithm/OptTimeCalculator.py | 24 ++++++++--------- 5 files changed, 41 insertions(+), 42 deletions(-) diff --git a/params/operator_params.json b/params/operator_params.json index 5ed6bea..db173a2 100644 --- a/params/operator_params.json +++ b/params/operator_params.json @@ -1,5 +1,5 @@ { - "dist_open_start_1": [ + "distance_h_start_1": [ 0.003, 0.003, 0.003, @@ -15,7 +15,7 @@ 0.003, 0.003 ], - "dist_open_start_2": [ + "distance_h_start_2": [ 0.005, 0.005, 0.005, @@ -31,7 +31,7 @@ 0.005, 0.005 ], - "dist_open_after_1": [ + "distance_s_1": [ 0.001, 0.001, 0.001, @@ -47,7 +47,7 @@ 0.001, 0.001 ], - "dist_open_after_2": [ + "distance_s_2": [ 0.001, 0.001, 0.001, @@ -63,7 +63,7 @@ 0.001, 0.001 ], - "dist_open_end_1": [ + "distance_l_1": [ 0.02, 0.02, 0.02, @@ -79,7 +79,7 @@ 0.02, 0.02 ], - "dist_open_end_2": [ + "distance_l_2": [ 0.033, 0.033, 0.033, @@ -95,7 +95,7 @@ 0.033, 0.033 ], - "dist_close_end_1": [ + "distance_h_end1": [ 0.003, 0.003, 0.003, @@ -111,7 +111,7 @@ 0.003, 0.003 ], - "dist_close_end_2": [ + "distance_h_end2": [ 0.005, 0.005, 0.005, diff --git a/params/system_params.json b/params/system_params.json index 1ac1dfc..d7949a9 100644 --- a/params/system_params.json +++ b/params/system_params.json @@ -41,10 +41,10 @@ "transmission_ratio_2": [ 1.0 ], - "position_start_1": [ + "contact_distance_1": [ 0.02 ], - "position_start_2": [ + "contact_distance_2": [ 0.081 ], "k_prop": [ diff --git a/src/OptAlgorithm/ConstantCalculator.py b/src/OptAlgorithm/ConstantCalculator.py index 6d073bb..498f089 100644 --- a/src/OptAlgorithm/ConstantCalculator.py +++ b/src/OptAlgorithm/ConstantCalculator.py @@ -14,8 +14,6 @@ class ConstantCalculator(AutoConfigClass): def calc(self): constants = {} - #self.smin1t = self.smin1 - self.dblock / 2 - #self.smin2t = self.smin2 - self.dblock / 2 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/OptAlgorithm.py b/src/OptAlgorithm/OptAlgorithm.py index 4299069..a2f1aa9 100644 --- a/src/OptAlgorithm/OptAlgorithm.py +++ b/src/OptAlgorithm/OptAlgorithm.py @@ -16,16 +16,18 @@ class OptAlgorithm(AutoConfigClass): calc = OptTimeCalculator(operator_config, system_config) - self.Ts = calc.T(self.dist_open_start_1, - self.dist_open_start_2, - self.dist_open_after_1, - self.dist_open_after_2, - self.dist_open_end_1, - self.dist_open_end_2) + self.Ts = calc.T(self.distance_h_start_1, + self.distance_h_start_2, + self.distance_s_1, + self.distance_s_2, + self.distance_l_1, + self.distance_l_2) - - self.x1Contact = self.dist_open_start_1 + self.position_start_1 - self.x2Contact = self.dist_open_start_2 + self.position_start_2 + self.x1_start = self.contact_distance_1 - self.distance_h_start_1 + self.x2_start = self.contact_distance_2 - self.distance_h_start_2 + + self.x1_contact = self.contact_distance_1 + self.x2_contact = self.contact_distance_2 self.pos0s, self.movementV0s = calc.Tmovement(self.getSpecific, self.getMarkOpen()) @@ -46,8 +48,7 @@ class OptAlgorithm(AutoConfigClass): t2 = max(t - self.Ts["tclose_1_acc"], 0) x1 = self.a_max_1 * self.Ts["tclose_1_acc"] * t2 - - return x0 + x1 + self.position_start_1 + return x0 + x1 + self.x1_start def V2Close(self, t: float): if t < self.Ts["tclose_2_acc"]: @@ -68,7 +69,7 @@ class OptAlgorithm(AutoConfigClass): t3 = max(min(t - self.Ts["tclose_2_speed"]- self.Ts["tclose_2_acc"], self.Ts["tclose_2_acc"]), 0) x2 = self.a_max_2 * self.Ts["tclose_2_acc"] * t3 - self.a_max_2 * t3 * t3 / 2 - return x0 + x1 + x2 + self.position_start_2 + return x0 + x1 + x2 + self.x2_start def FClose(self, t: float): return 0 @@ -95,7 +96,7 @@ class OptAlgorithm(AutoConfigClass): def X1Grow(self, t: float): F = self.FGrow(t) x = F / self.k_hardness_1 - return x + self.x1Contact + return x + self.x1_contact def V2Grow(self, t: float): """ @@ -109,7 +110,7 @@ class OptAlgorithm(AutoConfigClass): Считается, что верхний электрод не влияет на набор усилия, функция не реализована!, возвращает 0. Устанавливайте kturn = 0 """ - return self.x2Contact + return self.x2_contact def FGrow(self, t: float): v0 = self.a_max_1 * self.Ts["tclose_1_acc"] @@ -161,7 +162,7 @@ class OptAlgorithm(AutoConfigClass): t3 = max(min(t - self.Ts["topen_1_speed"]- self.Ts["topen_1_acc"], self.Ts["topen_1_acc"]), 0) x2 = -self.a_max_1 * self.Ts["topen_1_acc"] * t3 + self.a_max_1 * t3 * t3 / 2 - return xm + x0 + x1 + x2 + self.x1Contact + return xm + x0 + x1 + x2 + self.x1_contact def V2Open(self, t: float): @@ -187,18 +188,18 @@ class OptAlgorithm(AutoConfigClass): t3 = max(min(t - self.Ts["topen_2_speed"]- self.Ts["topen_2_acc"], self.Ts["topen_2_acc"]), 0) x2 = -self.a_max_2 * self.Ts["topen_2_acc"] * t3 + self.a_max_2 * t3 * t3 / 2 - return x0 + x1 + x2 + self.x2Contact + return x0 + x1 + x2 + self.x2_contact def FOpen(self, t: float): x1 = self.X1Open(t) x2 = self.X2Open(t) - F = self.k_hardness_1 * max(0, (x1 - self.x1Contact)) + F = self.k_hardness_1 * max(0, (x1 - self.x1_contact)) return F def FMovement(self, t: float): x1 = self.X1Movement(t) x2 = self.X2Movement(t) - F = self.k_hardness_1 * max(0, (x1 - self.x1Contact)) + F = 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 87d0e8c..e893334 100644 --- a/src/OptAlgorithm/OptTimeCalculator.py +++ b/src/OptAlgorithm/OptTimeCalculator.py @@ -19,7 +19,7 @@ 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 - t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 /2)) / v0q) + t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 /2)) / v0) T1 = t1 + t2t t21 = sqrt(h2/self.a_max_2) @@ -67,10 +67,10 @@ class OptTimeCalculator(AutoConfigClass): if s1 >= l1: raise Exception("""S1 >= L1 - недопустимый сценарий, - проверьте dist_open_after_1, dist_close_end_1""") + проверьте distance_s_1, distance_h_end1""") if s2 >= l2: raise Exception("""S2 >= L2 - недопустимый сценарий, - проверьте dist_open_after_2, dist_close_end_2""") + проверьте distance_s_2, distance_h_end2""") s1 += Fs1 topen_1_mark = sqrt(2 * s1 / self.a_max_1) @@ -134,23 +134,23 @@ class OptTimeCalculator(AutoConfigClass): return self.allTimes def Tmovement(self, closeAlgo, tmark) -> None: - contact = [self.dist_open_start_1 + self.position_start_1, self.dist_open_start_2 + self.position_start_2] + contact = [self.contact_distance_1, self.contact_distance_2] v0s = [] pos0s = [] for i in range(1,3): if tmark < 0: raise Exception("""Отрицательное время этапа раскрытия, - проверьте dist_open_after_{1,2}, time_command""") + проверьте distance_s_{1,2}, time_command""") v0 = closeAlgo("V"+str(i), "Open", tmark) v0s.append(v0) x0 = closeAlgo("X"+str(i), "Open", tmark) - x1 = contact[i-1] - self.__dict__["dist_close_end_"+str(i)] + x1 = contact[i-1] - self.__dict__["distance_h_end"+str(i)] x = x1 - x0 pos0s.append(closeAlgo("X"+str(i), "Open", tmark)) Tfull = self.time_robot_movement - L = self.__dict__["dist_open_end_"+str(i)] + L = self.__dict__["distance_l_"+str(i)] maxL = contact[i-1] - L - x0 self.Tmovementi(i, x, Tfull, v0, maxL) return pos0s, v0s @@ -164,7 +164,7 @@ class OptTimeCalculator(AutoConfigClass): 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} за указанное время, - проверьте dist_open_after_{i}, dist_close_end_{i}, time_command, time_robot_movement""") + проверьте 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))) @@ -208,25 +208,25 @@ class OptTimeCalculator(AutoConfigClass): 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 = sqrt(max(0, T**2 - 2 * s / self.a_max_1)) + 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 = sqrt(max(0, T * T - 4 * s / self.a_max_1)) + 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 = sqrt(max(0, T * T - 4 * s / self.a_max_2)) + t2 = max(0, (s - self.a_max_2*t1**2/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 = sqrt(max(0, T * T - 4 * s / self.a_max_2)) + t2 = max(0, (s - self.a_max_2*t1**2/2) / (self.a_max_2*t1)) return t1, t2 def calcSecondOpenOffset(self, t1 : float, t2 : float, sq : float) -> float: