303 lines
14 KiB
Python
303 lines
14 KiB
Python
import numpy as np
|
||
from numpy import sqrt, arcsin, arccos, cos, sin, array
|
||
|
||
from OptAlgorithm.AutoConfigClass import AutoConfigClass
|
||
from OptAlgorithm.ConstantCalculator import ConstantCalculator
|
||
|
||
|
||
class OptTimeCalculator(AutoConfigClass):
|
||
params_list = []
|
||
|
||
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 = {}
|
||
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)
|
||
t1 = v0 / self.a_max_1
|
||
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)
|
||
T1 = t1 + t2t + t3
|
||
|
||
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_wait, tclose_1_acc, tclose_1_speed, tclose_1_dec = 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_1_wait"] = tclose_1_wait
|
||
self.allTimes["tclose_1_dec"] = tclose_1_dec
|
||
|
||
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)
|
||
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)
|
||
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)
|
||
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_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:
|
||
s1 -= topen_1_acc ** 2 * self.a_max_1 / 2
|
||
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(max(0,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(max(0,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)
|
||
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
|
||
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)
|
||
ap = Fstart_prop / self.mass_1
|
||
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:
|
||
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):
|
||
contact = [self.contact_distance_1, self.contact_distance_2]
|
||
v0s = []
|
||
pos0s = []
|
||
for i in range(1, 3):
|
||
if tmark < 0:
|
||
raise Exception("""Отрицательное время этапа раскрытия,
|
||
проверьте 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__["distance_h_end" + str(i)]
|
||
x = x1 - x0
|
||
pos0s.append(closeAlgo("X" + str(i), "Open", tmark))
|
||
Tfull = self.time_robot_movement
|
||
|
||
L = self.__dict__["distance_l_" + str(i)]
|
||
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)]
|
||
Sfull = -Sfull
|
||
|
||
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
|
||
|
||
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)
|
||
|
||
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
|
||
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):
|
||
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)
|
||
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 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
|
||
t4 = 0
|
||
else:
|
||
t1 = 0
|
||
t2 = min(v0q / self.a_max_1, (T + v0/self.a_max_1)/2 - sqrt(max(0, (T + v0/self.a_max_1)**2 - 4 * (v0**2 / (2 * self.a_max_1**2) + s / self.a_max_1))) / 2)
|
||
v1 = t2 * self.a_max_1
|
||
t4 = max(0, v1-v0) / self.a_max_1
|
||
t3 = max(0, T - t2 - t4)
|
||
return t1, t2, t3, t4
|
||
|
||
def calcFirstOpen(self, T: float, s: 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):
|
||
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):
|
||
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:
|
||
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)
|
||
else:
|
||
offset = t1 + s / v1
|
||
return offset
|
||
|
||
|
||
|