1NiM-WatingRoom-WatchController/tools/trajectoryGen/utils/generator.py

if __name__ == "__main__": # If the Helper.py file is run directly
    print("This is a modul and should be imported")

import math

class Generator:
    dT = 0 
    aMax = 0
    wMax = 0 # v = a * t
    # s = a * t^2 / 2
    # s = v * t

    def __init__(self, stpr, dt, a, v):
        self.stepper = stpr
        self.dT = dt
        self.aMax = a
        self.wMax = v

    def __initVars(self):
        self.__pArr = [0]
        self.__wArr = [0]
        self.__tArr = [0]
        self.__t = 0

    def __appendPoint(self, p, w, t):
        self.__wArr.append(w)
        self.__pArr.append(p)
        self.__tArr.append(t)

    def __accelUntil(self, a, wInit, check):
        t = 0
        pX = self.__pArr[-1]
        while True:
            w = wInit + a * t
            p = pX + wInit * t + a * t**2 / 2
            if check(w, p):
                break
            self.__appendPoint(p, w, self.__t + t)
            t = t + self.dT
        self.__t = self.__t + t

    def __getStopDistance(self, a, w):
        tDec = w / a
        pDec = a * tDec**2 / 2
        return (pDec, tDec)

    def __moveUntil(self, wM, pEnd): 
        t = 0
        pX = self.__pArr[-1]
        while True:
            p = pX + wM * t
            if p > pEnd:
                break
            self.__appendPoint(p, wM, self.__t + t)
            t = t + self.dT
        self.__t = self.__t + t

    def __buildUpSwing(self, a, vM, swings):
        t = 0
        pX = self.__pArr[-1]

        TwMax = vM / a
        tau = 2*math.pi
        sTau = swings*tau

        while True:
            t = t + self.dT
            env = a * t
            p = math.sin(sTau*t/TwMax) * env * TwMax/sTau
            w = (p-self.__pArr[-1])/self.dT
            if w > self.wMax:
                break 
            self.__appendPoint(p, w, self.__t + t)

        self.__t = self.__t + t

    def __transformToStepper(self) :
        stp = self.stepper.StepRad
        res = stp / self.stepper.Resolution
        oArr = map(lambda x : int((x % stp) / res), self.__pArr)
        oArr = list(oArr)
        return {"p": self.__pArr, "w": self.__wArr, "t": self.__tArr, "pE": self.__pArr[-1], "T": self.__t, "o": oArr}

    def __smoothSubStep(self, delta, aM, wM):
        (pDec,_) = self.__getStopDistance(aM, wM)

        if (pDec < (delta - self.__pArr[-1])/2):
            # t0..t1: acceleration to max velocity
            self.__accelUntil(aM, 0, lambda w,p : w > wM)

            # t1..t2: movement to until almost destination
            self.__moveUntil(wM, delta - pDec)

            # t2..r3: deceleration until destination
            self.__accelUntil(-aM, wM, lambda w,p : w < 0)
        else:
            # t0..t1: acceleration until center is reached
            pCen = ((delta - self.__pArr[-1]) /2 + self.__pArr[-1]) 
            self.__accelUntil(aM, 0, lambda w,p : p > pCen)

            # t2..r3: deceleration until destination
            self.__accelUntil(-aM, self.__wArr[-1], lambda w,p : w < 0)

    def createSmoothStep(self, delta):
        self.__initVars()

        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)

        if (pDec < delta/2):
            # t0..t1: acceleration to max velocity
            self.__accelUntil(self.aMax, 0, lambda w,p : w > self.wMax)

            # t1..t2: movement to until almost destination
            self.__moveUntil(self.wMax, delta - pDec)

            # t2..r3: deceleration until destination
            self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)
        else:
            # t0..t1: acceleration until center is reached
            pCen = (self.__pArr[-1]) + delta /2
            self.__accelUntil(self.aMax, 0, lambda w,p : p > pCen)

            # t2..r3: deceleration until destination
            self.__accelUntil(-self.aMax, self.__wArr[-1] , lambda w,p : w < 0)

        return self.__transformToStepper()    
    
    def createBuildUpStep(self, delta): 
        self.__initVars()

        # t0..t1: increasing swing
        self.__buildUpSwing(self.aMax/4, self.wMax, 3)
        
        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)

        # t1..t2: movement to until almost destination
        self.__moveUntil(self.wMax, delta - pDec)
        
        # t2..r3: deceleration until destination
        self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)
        
        return self.__transformToStepper()

    def createRapidStartSwingInEnd(self, delta):
        self.__initVars()

        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)

        # t0..t1: acceleration to max velocity
        self.__accelUntil(self.aMax*2, 0, lambda w,p : w > self.wMax*2)
        
        # t1..r2: deceleration until moving speed
        self.__accelUntil(-self.aMax, self.wMax*2, lambda w,p : w < self.wMax )

        # t2..t3: movement to until destination (override)
        self.__moveUntil(self.wMax, delta)

        # t3..t4: deceleration
        self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)

        pCen = delta + (self.__pArr[-1] - delta) /2
        self.__accelUntil(-self.aMax, 0, lambda w,p : p < pCen )
        self.__accelUntil(self.aMax, self.__wArr[-1], lambda w,p : w > 0)

        return self.__transformToStepper()

    def createJumpInTarget(self, delta):
        self.__initVars()

        jumpInRange = math.pi/3
        deltaPre = delta - jumpInRange
        
        self.__smoothSubStep(deltaPre, self.aMax, self.wMax)
        
        jumpInSteps = [jumpInRange/2, jumpInRange/3, jumpInRange/6]
        nextPoint = deltaPre
        for step in jumpInSteps:
            nextPoint = nextPoint + step
            self.__smoothSubStep(nextPoint, self.aMax*10, self.wMax*2)
        return self.__transformToStepper()