Exemple #1
0
 def __init__(self):
     if Shiny.__red_bar is None:
         Shiny.__red_bar = load(template)
         # bar = svg[3, 0]
     self.attr = Shiny.__red_bar.attr
     self.colors = ["red", "blue", "green",
                    "red", "blue", "green"]
     self.bgcolor = "ffffff"
     self.screencolor = "ffffff"
     BaseGenerator.__init__(self)
Exemple #2
0
    def __init__(self,
                 genrator=BaseGenerator(),
                 QPsamplingPeriod=0.1,
                 NbSamplingPreviewed=16,
                 commandPeriod=0.005,
                 FeetDistance=0.2,
                 StepHeight=0.05,
                 stepTime=0.8,
                 doubleSupportTime=0.1):

        self.T = QPsamplingPeriod  # QP sampling period
        self.Tc = commandPeriod  # Low level control period
        self.feetDist = FeetDistance  # Normal Distance between both feet
        self.stepHeigth = StepHeight  # Standard maximal step height
        self.polynomeX = Polynome5()  # order 5 polynome for continuity
        self.polynomeY = Polynome5()  #  in position, velocity
        self.polynomeQ = Polynome5()  #  and acceleration
        self.polynomeZ = Polynome4()  # order 5 for a defined middle point
        self.TSS = stepTime - doubleSupportTime  # Time of single support
        self.TDS = doubleSupportTime  # Time of double support
        self.stepTime = stepTime
        self.intervaleSize = int(self.T /
                                 self.Tc)  # nuber of interpolated sample
        self.gen = genrator
        self.polynomeZ.setParameters(self.TSS, self.stepHeigth, 0.0, 0.0)
Exemple #3
0
    def __init__(self, Tc=0.005, BG=BaseGenerator()):

        # the generator is supposed to have been initialized before
        self.gen = BG

        self.T = self.gen.T  # QP sampling period
        self.Tc = Tc  # sampling period of the robot low level controller
        self.interval = int(self.T / self.Tc)  # number of iteration in 100ms
        # the initial state of the next QP iteration

        # initiale states used to interpolate (they should be intialized once at
        # the beginning of the qp
        # and updated inside the class
        self.curCoM = CoMState()
        self.curCoM.x = deepcopy(self.gen.c_k_x)
        self.curCoM.y = deepcopy(self.gen.c_k_y)
        self.curCoM.theta = deepcopy(self.gen.c_k_q)
        self.curCoM.h_com = deepcopy(self.gen.h_com)
        zmp = ZMPState()
        zmp.x = self.curCoM.x[
            0] - self.curCoM.h_com / self.gen.g * self.curCoM.x[2]
        zmp.y = self.curCoM.y[
            0] - self.curCoM.h_com / self.gen.g * self.curCoM.y[2]

        self.fi = FootInterpolation()
        self.curleft = BaseTypeFoot()
        self.curRight = BaseTypeFoot()
        if self.gen.currentSupport.foot == "left":
            self.curleft.x = deepcopy(self.gen.f_k_x)
            self.curleft.y = deepcopy(self.gen.f_k_y)
            self.curleft.q = deepcopy(self.gen.f_k_q)
            self.curRight.x = deepcopy(self.gen.f_k_x + self.fi.feetDist *
                                       math.sin(self.gen.f_k_q))
            self.curRight.y = deepcopy(self.gen.f_k_y - self.fi.feetDist *
                                       math.cos(self.gen.f_k_q))
            self.curRight.q = deepcopy(self.gen.f_k_q)
        else:
            self.curleft.x = deepcopy(self.gen.f_k_x)
            self.curleft.y = deepcopy(self.gen.f_k_y)
            self.curleft.q = deepcopy(self.gen.f_k_q)
            self.curRight.x = deepcopy(self.gen.f_k_x - self.fi.feetDist *
                                       math.sin(self.gen.f_k_q))
            self.curRight.y = deepcopy(self.gen.f_k_y + self.fi.feetDist *
                                       math.cos(self.gen.f_k_q))
            self.curRight.q = deepcopy(self.gen.f_k_q)

        self.CoMbuffer = numpy.empty(
            (self.interval, ),
            dtype=CoMState)  #buffer containing the CoM trajectory over 100ms
        self.ZMPbuffer = numpy.empty(
            (self.interval, ),
            dtype=ZMPState)  #buffer containing the ZMP trajectory over 100ms
        self.RFbuffer = numpy.empty(
            (self.interval, ), dtype=BaseTypeFoot
        )  #buffer containing the rigth foot trajectory over 100ms
        self.LFbuffer = numpy.empty(
            (self.interval, ), dtype=BaseTypeFoot
        )  #buffer containing the left foot trajectory over 100ms

        for i in range(self.interval):
            self.CoMbuffer[i] = deepcopy(self.curCoM)
            self.ZMPbuffer[i] = deepcopy(zmp)
            self.RFbuffer[i] = deepcopy(self.curRight)
            self.LFbuffer[i] = deepcopy(self.curleft)

        self.comTraj = []  #buffer containing the full CoM trajectory
        self.zmpTraj = []  #buffer containing the full ZMP trajectory
        self.leftFootTraj = [
        ]  #buffer containing the full left foot trajectory
        self.rightFootTraj = [
        ]  #buffer containing the full right foot trajectory

        for i in range(30):
            self.comTraj.extend(deepcopy(self.CoMbuffer))
            self.zmpTraj.extend(deepcopy(self.ZMPbuffer))
            self.leftFootTraj.extend(deepcopy(self.LFbuffer))
            self.rightFootTraj.extend(deepcopy(self.RFbuffer))

        self.lipm = LIPM(self.T, self.Tc, self.curCoM.h_com)
        self.fi = FootInterpolation(genrator=self.gen)
Exemple #4
0
 def __init__(self):
     BaseGenerator.__init__(self)
     self.calc = GeneratorCalc()