feat(SF-471): Добавлена фаза закрытия

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_1" : 0.006,
-    "dist_open_after_2" : 0.01,
+    "dist_open_after_2" : 0.006,
-    "dist_open_end_1" : 0.015,
+    "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

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)
 
-        

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

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
+        
+