From 11be1685a7584ac02fc9c36a84561f99455114b0 Mon Sep 17 00:00:00 2001 From: ermolaev_p Date: Thu, 7 Nov 2024 14:54:48 +0300 Subject: [PATCH] =?UTF-8?q?feat(SF-471):=20=D0=94=D0=BE=D0=B1=D0=B0=D0=B2?= =?UTF-8?q?=D0=BB=D0=B5=D0=BD=D0=B0=20=D1=84=D0=B0=D0=B7=D0=B0=20=D0=B7?= =?UTF-8?q?=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 --- params/operator_params.json | 10 +- src/OptAlgorithm/OptAlgorithm.py | 135 ++++++++++++++++++++++++-- src/OptAlgorithm/OptTimeCalculator.py | 90 ++++++++++++++--- src/OptAlgorithm/PhaseCalc.py | 38 ++++++++ 4 files changed, 249 insertions(+), 24 deletions(-) create mode 100644 src/OptAlgorithm/PhaseCalc.py diff --git a/params/operator_params.json b/params/operator_params.json index 08c755f..fcb6da4 100644 --- a/params/operator_params.json +++ b/params/operator_params.json @@ -1,11 +1,15 @@ { "dist_open_start_1" : 0.005, "dist_open_start_2" : 0.005, - "dist_open_after_1" : 0.01, - "dist_open_after_2" : 0.01, - "dist_open_end_1" : 0.015, + "dist_open_after_1" : 0.006, + "dist_open_after_2" : 0.006, + "dist_open_end_1" : 0.010, "dist_open_end_2" : 0.050, + "dist_close_end_1" : 0.005, + "dist_close_end_2" : 0.005, "time_wielding" : 2, + "time_command" : 0.060, + "time_robot_movement" : 0.2, "object_thickness" : 4.5e-3, "force_target" : 5000, "force_capture" : 500 diff --git a/src/OptAlgorithm/OptAlgorithm.py b/src/OptAlgorithm/OptAlgorithm.py index 633efe5..efd1938 100644 --- a/src/OptAlgorithm/OptAlgorithm.py +++ b/src/OptAlgorithm/OptAlgorithm.py @@ -1,3 +1,4 @@ +from src.OptAlgorithm.PhaseCalc import PhaseCalc from src.OptAlgorithm.OptTimeCalculator import OptTimeCalculator from src.OptAlgorithm.AutoConfigClass import AutoConfigClass from src.OptAlgorithm.ConstantCalculator import ConstantCalculator @@ -21,12 +22,17 @@ class OptAlgorithm(AutoConfigClass): self.dist_open_after_2, self.dist_open_end_1, self.dist_open_end_2) + self.x1Contact = self.dist_open_start_1 + self.position_start_1 self.x2Contact = self.dist_open_start_2 + self.position_start_2 + + self.pos0s, self.movementV0s = calc.Tmovement(self.getSpecific, self.getMarkOpen()) + + self.INF = 1e9 def getMarkOpen(self): - return max(self.Ts["topen_2_mark"], self.Ts["topen_1_mark"]) + return max(self.Ts["topen_2_mark"], self.Ts["topen_1_mark"]) - self.time_command def V1Close(self, t: float): if t < self.Ts["tclose_1_acc"]: @@ -188,7 +194,113 @@ class OptAlgorithm(AutoConfigClass): F = self.k_hardness_1 * max(0, (x1 - self.x1Contact)) return F - def calcPhaseClose(self, t: float): + def FMovement(self, t: float): + x1 = self.X1Movement(t) + x2 = self.X2Movement(t) + F = self.k_hardness_1 * max(0, (x1 - self.x1Contact)) + return F + + def X1Movement(self, t: float): + if "x1mov" in self.__dict__.keys(): + return self.x1mov(t) + self.x1mov = PhaseCalc(cummulative=True) + xacc = lambda t: self.movementV0s[0] * t - self.a_max_1 * t * t /2 + v1 = self.movementV0s[0] - self.Ts["tmovement_1_acc"] * self.a_max_1 + xspeed = lambda t: v1 * t + xslow = lambda t: v1 * t + self.a_max_1 * t * t /2 + xstay = lambda t: 0 + xcslow = lambda t: self.a_max_1 * t * t /2 + v3 = (self.Ts["tpreclose_1_slow"]) * self.a_max_1 + xcspeed = lambda t: v3 * t + xcacc = lambda t: v3 * t - self.a_max_1 * t * t /2 + xend = lambda t: 0 + xstart = lambda t: self.pos0s[0] + self.x1mov.add_phase(0, xstart) + self.x1mov.add_phase(self.Ts["tmovement_1_acc"], xacc) + self.x1mov.add_phase(self.Ts["tmovement_1_speed"], xspeed) + self.x1mov.add_phase(self.Ts["tmovement_1_slow"], xslow) + self.x1mov.add_phase(self.Ts["tmovement_1_stay"], xstay) + self.x1mov.add_phase(self.Ts["tpreclose_1_slow"], xcslow) + self.x1mov.add_phase(self.Ts["tpreclose_1_speed"], xcspeed) + self.x1mov.add_phase(self.Ts["tpreclose_1_acc"], xcacc) + self.x1mov.add_phase(self.INF, xend) + return self.x1mov(t) + + def V1Movement(self, t: float): + if "v1mov" in self.__dict__.keys(): + return self.v1mov(t) + self.v1mov = PhaseCalc(cummulative=False) + xacc = lambda t: self.movementV0s[0] - self.a_max_1 * t + v1 = self.movementV0s[0] - self.Ts["tmovement_1_acc"] * self.a_max_1 + xspeed = lambda t: v1 + xslow = lambda t: v1 + self.a_max_1 * t + xstay = lambda t: 0 + xcslow = lambda t: self.a_max_1 * t + v3 = (self.Ts["tpreclose_1_slow"]) * self.a_max_1 + xcspeed = lambda t: v3 + xcacc = lambda t: v3 - self.a_max_1 * t + xend = lambda t: 0 + self.v1mov.add_phase(self.Ts["tmovement_1_acc"], xacc) + self.v1mov.add_phase(self.Ts["tmovement_1_speed"], xspeed) + self.v1mov.add_phase(self.Ts["tmovement_1_slow"], xslow) + self.v1mov.add_phase(self.Ts["tmovement_1_stay"], xstay) + self.v1mov.add_phase(self.Ts["tpreclose_1_slow"], xcslow) + self.v1mov.add_phase(self.Ts["tpreclose_1_speed"], xcspeed) + self.v1mov.add_phase(self.Ts["tpreclose_1_acc"], xcacc) + self.v1mov.add_phase(self.INF, xend) + return self.v1mov(t) + + def X2Movement(self, t: float): + if "x2mov" in self.__dict__.keys(): + return self.x2mov(t) + self.x2mov = PhaseCalc(cummulative=True) + xacc = lambda t: self.movementV0s[1] * t - self.a_max_2 * t * t /2 + v1 = self.movementV0s[1] - self.Ts["tmovement_2_acc"] * self.a_max_2 + xspeed = lambda t: v1 * t + xslow = lambda t: v1 * t + self.a_max_2 * t * t /2 + xstay = lambda t: 0 + xcslow = lambda t: self.a_max_2 * t * t /2 + v3 = (self.Ts["tpreclose_2_slow"]) * self.a_max_2 + xcspeed = lambda t: v3 * t + xcacc = lambda t: v3 * t - self.a_max_2 * t * t /2 + xend = lambda t: 0 + xstart = lambda t: self.pos0s[1] + self.x2mov.add_phase(0, xstart) + self.x2mov.add_phase(self.Ts["tmovement_2_acc"], xacc) + self.x2mov.add_phase(self.Ts["tmovement_2_speed"], xspeed) + self.x2mov.add_phase(self.Ts["tmovement_2_slow"], xslow) + self.x2mov.add_phase(self.Ts["tmovement_2_stay"], xstay) + self.x2mov.add_phase(self.Ts["tpreclose_2_slow"], xcslow) + self.x2mov.add_phase(self.Ts["tpreclose_2_speed"], xcspeed) + self.x2mov.add_phase(self.Ts["tpreclose_2_acc"], xcacc) + self.x2mov.add_phase(self.INF, xend) + return self.x2mov(t) + + def V2Movement(self, t: float): + if "v2mov" in self.__dict__.keys(): + return self.v2mov(t) + self.v2mov = PhaseCalc(cummulative=False) + xacc = lambda t: self.movementV0s[1] - self.a_max_2 * t + v1 = self.movementV0s[1] - self.Ts["tmovement_2_acc"] * self.a_max_2 + xspeed = lambda t: v1 + xslow = lambda t: v1 + self.a_max_2 * t + xstay = lambda t: 0 + xcslow = lambda t: self.a_max_2 * t + v3 = (self.Ts["tpreclose_2_slow"]) * self.a_max_2 + xcspeed = lambda t: v3 + xcacc = lambda t: v3 - self.a_max_2 * t + xend = lambda t: 0 + self.v2mov.add_phase(self.Ts["tmovement_2_acc"], xacc) + self.v2mov.add_phase(self.Ts["tmovement_2_speed"], xspeed) + self.v2mov.add_phase(self.Ts["tmovement_2_slow"], xslow) + self.v2mov.add_phase(self.Ts["tmovement_2_stay"], xstay) + self.v2mov.add_phase(self.Ts["tpreclose_2_slow"], xcslow) + self.v2mov.add_phase(self.Ts["tpreclose_2_speed"], xcspeed) + self.v2mov.add_phase(self.Ts["tpreclose_2_acc"], xcacc) + self.v2mov.add_phase(self.INF, xend) + return self.v2mov(t) + + def calcPhaseClose(self, t: float) -> tuple[float, float, float, float, float]: """ Получить значения X1, X2, V1, V2, F в момент времени t для фазы смыкания @@ -200,7 +312,7 @@ class OptAlgorithm(AutoConfigClass): """ return self.X1Close(t), self.X2Close(t), self.V1Close(t), self.V2Close(t), self.FClose(t) - def calcPhaseGrow(self, t: float): + def calcPhaseGrow(self, t: float) -> tuple[float, float, float, float, float]: """ Получить значения X1, X2, V1, V2, F в момент времени t для фазы роста усилия @@ -212,7 +324,7 @@ class OptAlgorithm(AutoConfigClass): """ return self.X1Grow(t), self.X2Grow(t), self.V1Grow(t), self.V2Grow(t), self.FGrow(t) - def calcPhaseOpen(self, t: float): + def calcPhaseOpen(self, t: float) -> tuple[float, float, float, float, float]: """ Получить значения X1, X2, V1, V2, F в момент времени t для фазы раксрытия @@ -224,13 +336,13 @@ class OptAlgorithm(AutoConfigClass): """ return self.X1Open(t), self.X2Open(t), self.V1Open(t), self.V2Open(t), self.FOpen(t) - def getSpecific(self, param : str, phase : str, t : float): + def getSpecific(self, param : str, phase : str, t : float) -> float: """ Получить значение величины в определенную фазу в момент времени t (с начала фазы) Args: param (str): Значение из списка X1 | X2 | V1 | V2 | F - phase (str): Значение из списка: Close | Grow | Open + phase (str): Значение из списка: Close | Grow | Open | Movement t (float): Время Returns: @@ -244,7 +356,7 @@ class OptAlgorithm(AutoConfigClass): return 0 return func(t) - def getVar(self, param : str, t : float): + def getVar(self, param : str, t : float) -> float: """ Получить значение величины в момент времени t (БЕЗ УЧЕТА СВАРКИ!) @@ -261,6 +373,11 @@ class OptAlgorithm(AutoConfigClass): if t < self.Ts["tgrow"] : return self.getSpecific(param, "Grow", t) t -= self.Ts["tgrow"] - return self.getSpecific(param, "Open", t) + tmark = self.getMarkOpen() + if t < tmark: + return self.getSpecific(param, "Open", t) + t -= tmark + if t < self.Ts["tmovement"] : + return self.getSpecific(param, "Movement", t) + return self.getSpecific(param, "Movement", t) - \ No newline at end of file diff --git a/src/OptAlgorithm/OptTimeCalculator.py b/src/OptAlgorithm/OptTimeCalculator.py index be59605..aa1d9ce 100644 --- a/src/OptAlgorithm/OptTimeCalculator.py +++ b/src/OptAlgorithm/OptTimeCalculator.py @@ -12,10 +12,10 @@ class OptTimeCalculator(AutoConfigClass): super().__init__(OptTimeCalculator.params_list, operator_config, system_config, cCalculator.calc()) self.allTimes = {} - def tGrowNominal(self, F : float): + 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): + 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) t1 = v0 / self.a_max_1 @@ -39,7 +39,7 @@ class OptTimeCalculator(AutoConfigClass): 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): + 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) @@ -51,8 +51,8 @@ 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) offset = self.calcSecondOpenOffset(topen_1_acc, topen_1_speed, Fs1) @@ -97,7 +97,7 @@ class OptTimeCalculator(AutoConfigClass): self.allTimes["topen_2_mark"] = topen_2_mark self.allTimes["topen"] = Topen - def Tgrow(self): + def Tgrow(self) -> None: v0 = self.allTimes["tclose_1_acc"] * self.a_max_1 vF0 = v0 * self.k_hardness_1 @@ -122,37 +122,104 @@ class OptTimeCalculator(AutoConfigClass): 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): + 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: + contact = [self.dist_open_start_1 + self.position_start_1, self.dist_open_start_2 + self.position_start_2] + v0s = [] + pos0s = [] + for i in range(1,3): + tq = tmark + assert tq > 0 + v0 = closeAlgo("V"+str(i), "Open", tq) + v0s.append(v0) + x0 = closeAlgo("X"+str(i), "Open", tq) + x1 = contact[i-1] - self.__dict__["dist_close_end_"+str(i)] + x = x1 - x0 + pos0s.append(closeAlgo("X"+str(i), "Open", tq)) + Tfull = self.time_robot_movement + + + L = self.__dict__["dist_open_end_"+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 - def calcFirstClose(self, T : float, s : float): + sqrtval = a**2 * (a**2 * (Tfull+2*t3)**2 - 8 * a * Sfull + 2 * a* v0 * (Tfull+2*t3) - 3 *v0**2) + assert sqrtval >= 0 + 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 + S2max = abs(Sfull) + abs(S1) + 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) + 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 + + 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 = 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]: t1 = T - sqrt(max(0, T**2 - 2 * s / self.a_max_1)) t1 = min(t1, self.v_max_1 / self.a_max_1) t2 = sqrt(max(0, T**2 - 2 * s / self.a_max_1)) return t1, t2 - def calcFirstOpen(self, T : float, s : float): + 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)) return t1, t2 - def calcSecondOpen(self, T : float, s : float): + 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)) return t1, t2 - def calcSecondClose(self, T : float, s : float): + 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)) return t1, t2 - def calcSecondOpenOffset(self, t1 : float, t2 : float, sq : float): + def calcSecondOpenOffset(self, t1 : float, t2 : float, sq : float) -> float: s = sq * 1 offset = sqrt(2 * s / self.a_max_1) @@ -162,7 +229,6 @@ class OptTimeCalculator(AutoConfigClass): if s > t2 * v1: s -= t2 * v1 - print(s, t1 ** 2 * self.a_max_1/2) offset = 2*t1 + t2 - sqrt(t1**2 - 2*s / self.a_max_1) else: offset = t1 + s / v1 diff --git a/src/OptAlgorithm/PhaseCalc.py b/src/OptAlgorithm/PhaseCalc.py new file mode 100644 index 0000000..b55e733 --- /dev/null +++ b/src/OptAlgorithm/PhaseCalc.py @@ -0,0 +1,38 @@ + + +class PhaseCalc(): + + def __init__(self, cummulative = False): + + self.cummulative = cummulative + self.phases = [] + self.tims = [] + + def add_phase(self, t, func): + self.phases.append(func) + self.tims.append(t) + + def __call__(self, t): + if self.cummulative: + i = 0 + cumt = 0 + val = 0 + while self.tims[i] < t - cumt: + curt = min(t - cumt, self.tims[i]) + val += self.phases[i](curt) + cumt += self.tims[i] + i += 1 + val += self.phases[i](t - cumt) + return val + else: + i = 0 + cumt = 0 + val = 0 + while self.tims[i] < t - cumt: + curt = max(t - cumt, self.tims[i]) + cumt += self.tims[i] + i += 1 + val = self.phases[i](t - cumt) + return val + + \ No newline at end of file