Merge branch 'refs/heads/feature-positioning' into feature-client-trace-modifier
# Conflicts: # params/operator_params.json # params/system_params.json # requirements.txt # src/controller/passportFormer.py # src/gui/mainGui.py # src/gui/plotter.py # src/gui/settings_window.py # src/utils/base/base.py
This commit is contained in:
commit
f60672eb7c
@ -16,6 +16,12 @@ class OptAlgorithm(AutoConfigClass):
|
||||
|
||||
calc = OptTimeCalculator(operator_config, system_config)
|
||||
|
||||
if self.distance_l_1 < 0:
|
||||
raise Exception("Ошибка - заготовка вышла за пределы маскимального раскрытия электрода FE")
|
||||
|
||||
if self.distance_l_2 < 0:
|
||||
raise Exception("Ошибка - заготовка вышла за пределы маскимального раскрытия электрода ME")
|
||||
|
||||
self.Ts = calc.T(self.distance_h_start_1,
|
||||
self.distance_h_start_2,
|
||||
self.distance_s_1,
|
||||
@ -37,18 +43,40 @@ class OptAlgorithm(AutoConfigClass):
|
||||
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"]:
|
||||
return self.a_max_1 * t
|
||||
else:
|
||||
return self.a_max_1 * self.Ts["tclose_1_acc"]
|
||||
|
||||
def X1Close(self, t: float):
|
||||
t1 = min(t, self.Ts["tclose_1_acc"])
|
||||
x0 = self.a_max_1 * t1 * t1 / 2
|
||||
|
||||
t2 = max(t - self.Ts["tclose_1_acc"], 0)
|
||||
x1 = self.a_max_1 * self.Ts["tclose_1_acc"] * t2
|
||||
return x0 + x1 + self.x1_start
|
||||
if "v1close" in self.__dict__.keys():
|
||||
return self.v1close(t)
|
||||
self.v1close = PhaseCalc(cummulative=False)
|
||||
xwait = lambda t: 0
|
||||
xacc = lambda t: self.a_max_1 * t
|
||||
v1 = (self.Ts["tclose_1_acc"]) * self.a_max_1
|
||||
xspeed = lambda t: v1
|
||||
xdec = lambda t: v1 - self.a_max_1 * t
|
||||
xend = lambda t: 0
|
||||
self.v1close.add_phase(self.Ts["tclose_1_wait"], xwait)
|
||||
self.v1close.add_phase(self.Ts["tclose_1_acc"], xacc)
|
||||
self.v1close.add_phase(self.Ts["tclose_1_speed"], xspeed)
|
||||
self.v1close.add_phase(self.Ts["tclose_1_dec"], xdec)
|
||||
self.v1close.add_phase(self.INF, xend)
|
||||
return self.v1close(t)
|
||||
|
||||
def X1Close(self, T: float):
|
||||
if "x1close" in self.__dict__.keys():
|
||||
return self.x1close(T)
|
||||
self.x1close = PhaseCalc(cummulative=True)
|
||||
xwait = lambda t: 0
|
||||
xacc = lambda t: self.a_max_1 * t * t /2
|
||||
v1 = (self.Ts["tclose_1_acc"]) * self.a_max_1
|
||||
xspeed = lambda t: v1 * t
|
||||
xdec = lambda t: v1 * t - self.a_max_1 * t * t /2
|
||||
xstart = lambda t: self.x1_start
|
||||
xend = lambda t: 0
|
||||
self.x1close.add_phase(0, xstart)
|
||||
self.x1close.add_phase(self.Ts["tclose_1_wait"], xwait)
|
||||
self.x1close.add_phase(self.Ts["tclose_1_acc"], xacc)
|
||||
self.x1close.add_phase(self.Ts["tclose_1_speed"], xspeed)
|
||||
self.x1close.add_phase(self.Ts["tclose_1_dec"], xdec)
|
||||
self.x1close.add_phase(self.INF, xend)
|
||||
return self.x1close(T)
|
||||
|
||||
def V2Close(self, t: float):
|
||||
if t < self.Ts["tclose_2_acc"]:
|
||||
@ -199,7 +227,8 @@ class OptAlgorithm(AutoConfigClass):
|
||||
def FMovement(self, t: float):
|
||||
x1 = self.X1Movement(t)
|
||||
x2 = self.X2Movement(t)
|
||||
F = self.k_hardness_1 * max(0, (x1 - self.x1_contact))
|
||||
#its not true, but theres no need to track collisions at this point
|
||||
F = 0#self.k_hardness_1 * max(0, (x1 - self.x1_contact))
|
||||
return F
|
||||
|
||||
def X1Movement(self, t: float):
|
||||
|
||||
@ -1,4 +1,5 @@
|
||||
from numpy import sqrt, arcsin, arccos, cos, sin
|
||||
import numpy as np
|
||||
from numpy import sqrt, arcsin, arccos, cos, sin, array
|
||||
|
||||
from OptAlgorithm.AutoConfigClass import AutoConfigClass
|
||||
from OptAlgorithm.ConstantCalculator import ConstantCalculator
|
||||
@ -21,8 +22,12 @@ 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)) / v0)
|
||||
T1 = t1 + t2t
|
||||
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)
|
||||
@ -31,11 +36,13 @@ class OptTimeCalculator(AutoConfigClass):
|
||||
|
||||
Tclose = max(T1, T2)
|
||||
|
||||
tclose_1_acc, tclose_1_speed = self.calcFirstClose(Tclose, h1)
|
||||
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
|
||||
@ -80,7 +87,7 @@ 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(max(0,topen_1_acc ** 2 - 2 * s1 / self.a_max_1))
|
||||
else:
|
||||
topen_1_mark = topen_1_acc + s1 / v1
|
||||
|
||||
@ -90,7 +97,7 @@ class OptTimeCalculator(AutoConfigClass):
|
||||
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(max(0,topen_2_acc ** 2 - 2 * s2 / self.a_max_2))
|
||||
else:
|
||||
topen_2_mark = topen_2_acc + s2 / v2
|
||||
|
||||
@ -102,7 +109,7 @@ class OptTimeCalculator(AutoConfigClass):
|
||||
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)
|
||||
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) * (
|
||||
@ -136,7 +143,7 @@ class OptTimeCalculator(AutoConfigClass):
|
||||
self.Topen(s1, s2, l1, l2, self.force_target / self.k_hardness_1, 0)
|
||||
return self.allTimes
|
||||
|
||||
def Tmovement(self, closeAlgo, tmark) -> tuple[list, list]:
|
||||
def Tmovement(self, closeAlgo, tmark):
|
||||
contact = [self.contact_distance_1, self.contact_distance_2]
|
||||
v0s = []
|
||||
pos0s = []
|
||||
@ -153,47 +160,78 @@ class OptTimeCalculator(AutoConfigClass):
|
||||
Tfull = self.time_robot_movement
|
||||
|
||||
L = self.__dict__["distance_l_" + str(i)]
|
||||
maxL = contact[i - 1] - L - x0
|
||||
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)]
|
||||
t3 = (Tfull + v0 / a) / 2
|
||||
Sfull = -Sfull
|
||||
|
||||
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)))
|
||||
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("Ошибка - прохождение второй части перемещения - отрицательно, неизвестное поведение")
|
||||
|
||||
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 = Sfull + S1
|
||||
t5 = min(t5max, (vmax) / a, sqrt(S2max / a))
|
||||
t3 = abs(v0) / a + t1 + t5
|
||||
t32 = t5
|
||||
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
|
||||
|
||||
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
|
||||
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)
|
||||
|
||||
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)
|
||||
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
|
||||
@ -206,30 +244,39 @@ class OptTimeCalculator(AutoConfigClass):
|
||||
T = Tfull
|
||||
self.allTimes["tmovement"] = T
|
||||
|
||||
def calcFirstClose(self, T: float, s: float) -> tuple[float, float]:
|
||||
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)
|
||||
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
|
||||
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) -> tuple[float, float]:
|
||||
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) -> tuple[float, float]:
|
||||
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) -> tuple[float, float]:
|
||||
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, вообще не знаю как так получилось""")
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user