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)
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)
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)
def __init__(self):
BaseGenerator.__init__(self)
self.calc = GeneratorCalc()
