dev: обновил OptAlgorithm до последней версии + отдельные конфиги (системные и пользовательские)
This commit is contained in:
parent
692b437d2e
commit
e4afaff1c1
18
OptAlgorithm/AutoConfigClass.py
Normal file
18
OptAlgorithm/AutoConfigClass.py
Normal file
@ -0,0 +1,18 @@
|
||||
class AutoConfigClass():
|
||||
|
||||
def __init__(self, list : list[str], *configs : dict):
|
||||
self.valid = True
|
||||
for param in list:
|
||||
flag = 0
|
||||
for c in configs:
|
||||
if param in c.keys():
|
||||
flag += 1
|
||||
if flag == 0:
|
||||
self.valid = False
|
||||
raise BufferError("Not enough params to unpack: " + param + " not found")
|
||||
if flag > 1:
|
||||
self.valid = False
|
||||
raise BufferError("ParamDuplicate: " + param + " not found")
|
||||
for c in configs:
|
||||
for key, value in c.items():
|
||||
setattr(self, key, value)
|
||||
23
OptAlgorithm/ConstantCalculator.py
Normal file
23
OptAlgorithm/ConstantCalculator.py
Normal file
@ -0,0 +1,23 @@
|
||||
from OptAlgorithm.AutoConfigClass import AutoConfigClass
|
||||
from numpy import sqrt
|
||||
|
||||
class ConstantCalculator(AutoConfigClass):
|
||||
|
||||
params_list = []
|
||||
def __init__(self, operator_config : dict, system_config : dict):
|
||||
|
||||
super().__init__(ConstantCalculator.params_list, operator_config, system_config)
|
||||
|
||||
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
|
||||
constants["Ftogrow"] = self.force_target * (1 - self.k_prop)
|
||||
constants["Fstart_prop"] = 3 * self.force_target / 2 - 1/2 * sqrt(self.force_target*self.force_target + 16 * self.force_target * constants["Fprop"] - 8 * constants["Fprop"]**2)
|
||||
return constants
|
||||
263
OptAlgorithm/OptAlgorithm.py
Normal file
263
OptAlgorithm/OptAlgorithm.py
Normal file
@ -0,0 +1,263 @@
|
||||
from OptAlgorithm.OptTimeCalculator import OptTimeCalculator
|
||||
from OptAlgorithm.AutoConfigClass import AutoConfigClass
|
||||
from OptAlgorithm.ConstantCalculator import ConstantCalculator
|
||||
|
||||
from numpy import cos, sin, sqrt, cbrt, arcsin
|
||||
|
||||
|
||||
class OptAlgorithm(AutoConfigClass):
|
||||
|
||||
params_list = []
|
||||
|
||||
def __init__(self, operator_config : dict, system_config : dict):
|
||||
cCalculator = ConstantCalculator(operator_config, system_config)
|
||||
super().__init__(OptAlgorithm.params_list, operator_config, system_config, cCalculator.calc())
|
||||
|
||||
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.x1Contact = self.dist_open_start_1 + self.position_start_1
|
||||
self.x2Contact = self.dist_open_start_2 + self.position_start_2
|
||||
|
||||
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.position_start_1
|
||||
|
||||
def V2Close(self, t: float):
|
||||
if t < self.Ts["tclose_2_acc"]:
|
||||
return self.a_max_2 * t
|
||||
t -= self.Ts["tclose_2_acc"]
|
||||
if t < self.Ts["tclose_2_speed"]:
|
||||
return self.a_max_2 * self.Ts["tclose_2_acc"]
|
||||
t -= self.Ts["tclose_2_speed"]
|
||||
return self.a_max_2 * self.Ts["tclose_2_acc"] - self.a_max_2 * t
|
||||
|
||||
def X2Close(self, t: float):
|
||||
t1 = min(t, self.Ts["tclose_2_acc"])
|
||||
x0 = self.a_max_2 * t1 * t1 / 2
|
||||
|
||||
t2 = max(min(t - self.Ts["tclose_2_acc"], self.Ts["tclose_2_speed"]), 0)
|
||||
x1 = self.a_max_2 * self.Ts["tclose_2_acc"] * t2
|
||||
|
||||
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
|
||||
|
||||
def FClose(self, t: float):
|
||||
return 0
|
||||
|
||||
def V1Grow(self, t: float):
|
||||
F = self.FGrow(t)
|
||||
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)
|
||||
if t < self.Ts["tspeed"]:
|
||||
dF = sqrt(self.k_hardness_1 / self.mass_1) * self.eff_control * sin(sqrt(self.k_hardness_1 / self.mass_1) * t) + dF0 * cos(sqrt(self.k_hardness_1 / self.mass_1) * t)
|
||||
return dF / self.k_hardness_1
|
||||
t -= self.Ts["tspeed"]
|
||||
if t < self.Ts["tmeet"]:
|
||||
return dFmax / self.k_hardness_1
|
||||
t -= self.Ts["tmeet"]
|
||||
if t < self.Ts["tend"]:
|
||||
return sqrt(self.k_hardness_1/self.mass_1)*sqrt((self.force_target-self.Fprop)**2 - F**2) / self.k_hardness_1
|
||||
v0 = sqrt(self.k_hardness_1/self.mass_1)*sqrt((self.force_target-self.Fprop)**2 - self.Fstart_prop**2)
|
||||
b = (1/3 * v0 / cbrt((self.force_target - self.Fstart_prop))**2)**3
|
||||
dF = 3.0*b*cbrt((F -self.force_target)/b)**2
|
||||
return dF / self.k_hardness_1
|
||||
|
||||
def X1Grow(self, t: float):
|
||||
F = self.FGrow(t)
|
||||
x = F / self.k_hardness_1
|
||||
return x + self.x1Contact
|
||||
|
||||
def V2Grow(self, t: float):
|
||||
"""
|
||||
Считается, что верхний электрод не влияет на набор усилия,
|
||||
функция не реализована!, возвращает 0. Устанавливайте kturn = 0
|
||||
"""
|
||||
return 0
|
||||
|
||||
def X2Grow(self, t: float):
|
||||
"""
|
||||
Считается, что верхний электрод не влияет на набор усилия,
|
||||
функция не реализована!, возвращает 0. Устанавливайте kturn = 0
|
||||
"""
|
||||
return self.x2Contact
|
||||
|
||||
def FGrow(self, t: float):
|
||||
v0 = self.a_max_1 * self.Ts["tclose_1_acc"]
|
||||
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)
|
||||
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"]:
|
||||
return - self.eff_control * cos(self.freq * t) + self.eff_control + 1/self.freq * dF0 * sin(self.freq * t)
|
||||
t -= self.Ts["tspeed"]
|
||||
if t < self.Ts["tmeet"]:
|
||||
return Fspeed + t * dFmax
|
||||
t -= self.Ts["tmeet"]
|
||||
if t < self.Ts["tend"]:
|
||||
tm = arcsin(self.Fmeet/(self.Ftogrow)) * 1/self.freq
|
||||
return self.Ftogrow * sin(self.freq*(t + tm))
|
||||
t -= self.Ts["tend"]
|
||||
|
||||
#tprop
|
||||
a_max_1 = self.force_target
|
||||
v0 = self.freq*sqrt((self.force_target-self.Fprop)**2 - self.Fstart_prop**2)
|
||||
b = (1/3 * v0 / cbrt((self.force_target - self.Fstart_prop))**2)**3
|
||||
q = self.Fstart_prop
|
||||
|
||||
return 3 * t**2 * cbrt(b*b*(q-a_max_1)) + 3 * t * cbrt(q-a_max_1)**2 * cbrt(b) + b * t**3 + q
|
||||
|
||||
def V1Open(self, t: float):
|
||||
if t < self.Ts["topen_1_acc"]:
|
||||
return -self.a_max_1 * t
|
||||
t -= self.Ts["topen_1_acc"]
|
||||
if t < self.Ts["topen_1_speed"]:
|
||||
return -self.a_max_1 * self.Ts["topen_1_acc"]
|
||||
t -= self.Ts["topen_1_speed"]
|
||||
if t < self.Ts["topen_1_acc"]:
|
||||
return -self.a_max_1 * self.Ts["topen_1_acc"] + self.a_max_1 * t
|
||||
return 0
|
||||
|
||||
def X1Open(self, t: float):
|
||||
xm = self.force_target / self.k_hardness_1
|
||||
t1 = min(t, self.Ts["topen_1_acc"])
|
||||
x0 = -self.a_max_1 * t1 * t1 / 2
|
||||
|
||||
t2 = max(min(t - self.Ts["topen_1_acc"], self.Ts["topen_1_speed"]), 0)
|
||||
x1 = -self.a_max_1 * self.Ts["topen_1_acc"] * t2
|
||||
|
||||
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
|
||||
|
||||
def V2Open(self, t: float):
|
||||
|
||||
t = max(t-self.Ts["topen_2_offset"] , 0)
|
||||
if t < self.Ts["topen_2_acc"]:
|
||||
return -self.a_max_2 * t
|
||||
t -= self.Ts["topen_2_acc"]
|
||||
if t < self.Ts["topen_2_speed"]:
|
||||
return -self.a_max_2 * self.Ts["topen_2_acc"]
|
||||
t -= self.Ts["topen_2_speed"]
|
||||
if t < self.Ts["topen_2_acc"]:
|
||||
return -self.a_max_2 * self.Ts["topen_2_acc"] + self.a_max_2 * t
|
||||
return 0
|
||||
|
||||
def X2Open(self, t: float):
|
||||
t = max(t-self.Ts["topen_2_offset"] , 0)
|
||||
t1 = min(t, self.Ts["topen_2_acc"])
|
||||
x0 = -self.a_max_2 * t1 * t1 / 2
|
||||
|
||||
t2 = max(min(t - self.Ts["topen_2_acc"], self.Ts["topen_2_speed"]), 0)
|
||||
x1 = -self.a_max_2 * self.Ts["topen_2_acc"] * t2
|
||||
|
||||
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
|
||||
|
||||
def FOpen(self, t: float):
|
||||
x1 = self.X1Open(t)
|
||||
x2 = self.X2Open(t)
|
||||
F = self.k_hardness_1 * max(0, (x1 - self.x1Contact))
|
||||
return F
|
||||
|
||||
def calcPhaseClose(self, t: float):
|
||||
"""
|
||||
Получить значения X1, X2, V1, V2, F в момент времени t для фазы смыкания
|
||||
|
||||
Args:
|
||||
t (float): Момент времени
|
||||
|
||||
Returns:
|
||||
(float, float, float, float, float): X1, X2, V1, V2, F
|
||||
"""
|
||||
return self.X1Close(t), self.X2Close(t), self.V1Close(t), self.V2Close(t), self.FClose(t)
|
||||
|
||||
def calcPhaseGrow(self, t: float):
|
||||
"""
|
||||
Получить значения X1, X2, V1, V2, F в момент времени t для фазы роста усилия
|
||||
|
||||
Args:
|
||||
t (float): Момент времени
|
||||
|
||||
Returns:
|
||||
(float, float, float, float, float): X1, X2, V1, V2, F
|
||||
"""
|
||||
return self.X1Grow(t), self.X2Grow(t), self.V1Grow(t), self.V2Grow(t), self.FGrow(t)
|
||||
|
||||
def calcPhaseOpen(self, t: float):
|
||||
"""
|
||||
Получить значения X1, X2, V1, V2, F в момент времени t для фазы раксрытия
|
||||
|
||||
Args:
|
||||
t (float): Момент времени
|
||||
|
||||
Returns:
|
||||
(float, float, float, float, float): X1, X2, V1, V2, F
|
||||
"""
|
||||
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):
|
||||
"""
|
||||
Получить значение величины в определенную фазу в момент времени t (с начала фазы)
|
||||
|
||||
Args:
|
||||
param (str): Значение из списка X1 | X2 | V1 | V2 | F
|
||||
phase (str): Значение из списка: Close | Grow | Open
|
||||
t (float): Время
|
||||
|
||||
Returns:
|
||||
Значение величины
|
||||
"""
|
||||
funcName = param + phase
|
||||
try:
|
||||
func = getattr(self, funcName)
|
||||
except:
|
||||
print("Wrong param or phase name")
|
||||
return 0
|
||||
return func(t)
|
||||
|
||||
def getVar(self, param : str, t : float):
|
||||
"""
|
||||
Получить значение величины в момент времени t (БЕЗ УЧЕТА СВАРКИ!)
|
||||
|
||||
Args:
|
||||
param (str): Значение из списка X1 | X2 | V1 | V2 | F
|
||||
t (float): Время
|
||||
|
||||
Returns:
|
||||
Значение величины
|
||||
"""
|
||||
if t < self.Ts["tclose"]:
|
||||
return self.getSpecific(param, "Close", t)
|
||||
t -= self.Ts["tclose"]
|
||||
if t < self.Ts["tgrow"] :
|
||||
return self.getSpecific(param, "Grow", t)
|
||||
t -= self.Ts["tgrow"]
|
||||
return self.getSpecific(param, "Open", t)
|
||||
|
||||
|
||||
172
OptAlgorithm/OptTimeCalculator.py
Normal file
172
OptAlgorithm/OptTimeCalculator.py
Normal file
@ -0,0 +1,172 @@
|
||||
from numpy import sqrt, arcsin, arccos, cos, sin
|
||||
|
||||
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 = {}
|
||||
|
||||
def tGrowNominal(self, F : float):
|
||||
return arcsin(F/(self.Ftogrow)) * sqrt(self.mass_1/self.k_hardness_1)
|
||||
|
||||
def Tclose(self, h1: float, h2: float):
|
||||
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)
|
||||
T1 = t1 + t2t
|
||||
|
||||
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_acc, tclose_1_speed = 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_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):
|
||||
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 = max(T1, T2)
|
||||
topen_1_acc, topen_1_speed = self.calcFirstOpen(Topen, l1+Fs1)
|
||||
offset = self.calcSecondOpenOffset(topen_1_acc, topen_1_speed, Fs1)
|
||||
|
||||
topen_2_acc, topen_2_speed = self.calcSecondOpen(Topen - 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 ValueError("S1 > L1 - недопустимый сценарий")
|
||||
if s2 > l2:
|
||||
raise ValueError("S2 > L2 - недопустимый сценарий")
|
||||
|
||||
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(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(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
|
||||
self.allTimes["topen"] = Topen
|
||||
|
||||
def Tgrow(self):
|
||||
|
||||
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.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)
|
||||
Fstart_prop = self.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)
|
||||
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):
|
||||
self.Tclose(h1, h2)
|
||||
self.Tgrow()
|
||||
self.Topen(s1, s2, l1, l2, self.force_target / self.k_hardness_1, 0)
|
||||
return self.allTimes
|
||||
|
||||
def calcFirstClose(self, T : float, s : 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):
|
||||
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):
|
||||
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):
|
||||
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):
|
||||
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
|
||||
|
||||
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
|
||||
return offset
|
||||
|
||||
|
||||
|
||||
1
OptAlgorithm/__init__.py
Normal file
1
OptAlgorithm/__init__.py
Normal file
@ -0,0 +1 @@
|
||||
from .OptAlgorithm import OptAlgorithm
|
||||
2
UML.svg
2
UML.svg
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 17 KiB After Width: | Height: | Size: 26 KiB |
10
UML.txt
10
UML.txt
@ -63,14 +63,14 @@ CD is {-}
|
||||
CD is opening
|
||||
@300.0
|
||||
CD is {-}
|
||||
@18.32885128339674
|
||||
@56.27781200794141
|
||||
ID is {-}
|
||||
@18.42885128339674
|
||||
@56.37781200794141
|
||||
ID is compression #yellow
|
||||
@44.99013343875682
|
||||
@91.4562305565861
|
||||
ID is {-}
|
||||
@45.09013343875683
|
||||
@91.55623055658612
|
||||
ID is opening #yellow
|
||||
@91.75944699185315
|
||||
@276.6234655587717
|
||||
ID is {-}
|
||||
@enduml
|
||||
|
||||
17
main.py
17
main.py
@ -1,20 +1,18 @@
|
||||
from Requestinator import Request
|
||||
from DataParsinator import DataParser
|
||||
import pyqtgraph as pg
|
||||
from pyqtgraph.Qt import QtCore, QtGui
|
||||
import numpy as np
|
||||
|
||||
from utils import read_json
|
||||
from Requestinator import Request
|
||||
from DataParsinator import DataParser
|
||||
#нижний fe x1
|
||||
|
||||
|
||||
import qt_settings as qts
|
||||
from OptAlgorithm import OptAlgorithm
|
||||
|
||||
|
||||
class Application:
|
||||
def __init__(self):
|
||||
def __init__(self, opt: OptAlgorithm):
|
||||
pg.setConfigOptions(antialias=True)
|
||||
self.opt = OptAlgorithm()
|
||||
self.opt = opt
|
||||
self.scaler = 1000
|
||||
self.parser = DataParser(self.scaler)
|
||||
self.r = Request()
|
||||
@ -248,7 +246,10 @@ class Application:
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
app = Application()
|
||||
operator_params = read_json("params/operator_params.json")
|
||||
system_params = read_json("params/system_params.json")
|
||||
opt_algorithm = OptAlgorithm(operator_config=operator_params, system_config=system_params)
|
||||
app = Application(opt_algorithm)
|
||||
app.updatePlots()
|
||||
pg.exec()
|
||||
|
||||
|
||||
12
params/operator_params.json
Normal file
12
params/operator_params.json
Normal file
@ -0,0 +1,12 @@
|
||||
{
|
||||
"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_end_2" : 0.050,
|
||||
"time_wielding" : 2,
|
||||
"object_thickness" : 4.5e-3,
|
||||
"force_target" : 5000,
|
||||
"force_capture" : 500
|
||||
}
|
||||
18
params/system_params.json
Normal file
18
params/system_params.json
Normal file
@ -0,0 +1,18 @@
|
||||
{
|
||||
"a_max_1" : 5.41,
|
||||
"v_max_1" : 0.108,
|
||||
"a_max_2" : 35.81,
|
||||
"v_max_2" : 0.678,
|
||||
"mass_1" : 257,
|
||||
"mass_2" : 1,
|
||||
"k_hardness_1" : 1759291,
|
||||
"k_hardness_2" : 0,
|
||||
"torque_max_1" : 20,
|
||||
"torque_max_2" : 0,
|
||||
"transmission_ratio_1" : 0.00125,
|
||||
"transmission_ratio_2" : 1,
|
||||
"position_start_1" : 0.080,
|
||||
"position_start_2" : 0.080,
|
||||
"k_prop" : 0.05,
|
||||
"time_capture" : 100000
|
||||
}
|
||||
1
utils/__init__.py
Normal file
1
utils/__init__.py
Normal file
@ -0,0 +1 @@
|
||||
from .json_tools import read_json
|
||||
11
utils/json_tools.py
Normal file
11
utils/json_tools.py
Normal file
@ -0,0 +1,11 @@
|
||||
from os import path
|
||||
import json
|
||||
|
||||
|
||||
def read_json(filepath: str) -> dict:
|
||||
if not path.exists(filepath):
|
||||
raise FileNotFoundError(f"JSON file {filepath} not found!")
|
||||
|
||||
with open(filepath, 'r') as json_file:
|
||||
data = json.load(json_file)
|
||||
return data
|
||||
Loading…
Reference in New Issue
Block a user