plt.hold(True)
# ~ plt.plot(steps[0],steps[1],"d")
log_zmp_x.append(steps[0][0])
log_zmp_y.append(steps[1][0])
labels = ["p{0} - t{1}".format(k, i) for k in range(N)]
# ~ for label, x, y in zip(labels, steps[0], steps[1]):
# ~ plt.annotate(
# ~ label,
# ~ xy = (x, y), xytext = (-20, 20),
# ~ textcoords = 'offset points', ha = 'right', va = 'bottom',
# ~ bbox = dict(boxstyle = 'round,pad=0.5', fc = 'yellow', alpha = 0.5),
# ~ arrowprops = dict(arrowstyle = '->', connectionstyle = 'arc3,rad=0'))
[tt, cc_x, cc_y, d_cc_x, d_cc_y] = pg.computePreviewOfCom(steps, alpha, x0, 111)
# ~ print ('len =')
# ~ print len(tt)
# ~ plt.plot(cc_x,cc_y)
# ~ plt.figure(2)
# ~ plt.plot(tt,d_cc_y,'-x')
# ~ plt.plot([alpha*pg.durrationOfStep,alpha*pg.durrationOfStep],[-1,1])
# prepare next iteration
zmp = [steps[0][0], steps[1][0]]
tmp = pg.computeNextCom(zmp, x0, dt)
x0 = [[tmp[0], tmp[2]], [tmp[1], tmp[3]]]
alpha = alpha + 1 / 30.0
# ends the current step:
while(ev<1.0 and RUN_FLAG):
#time.sleep(1)
t=durrationOfStep*ev
#solve MPC for current state x
'''extract 1st command to apply, cop position and preview position of com'''
steps = pg.computeStepsPosition(ev,p0,v,x, LR)
cop=[steps[0][0],steps[1][0]]
[c_x , c_y , d_c_x , d_c_y] = pg.computeNextCom(cop,x,dt)
w2= pg.g/pg.h
dd_c_x = w2*( c_x - cop[0] )
dd_c_y = w2*( c_y - cop[1] )
x_cmd=[[c_x,d_c_x] , [c_y,d_c_y]] #command to apply
[tt, cc_x , cc_y , d_cc_x , d_cc_y] = pg.computePreviewOfCom(steps,ev,x,N=N_COM_TO_DISPLAY)
if ENABLE_LOGING:
for i in range(len(tt)):
tt[i]+=tk
#~ plt.subplot(2,2,1)
#~ plt.plot(tt,cc_x,'.') #actual preview
#~ plt.subplot(2,2,2)
#~ plt.plot(tt,cc_y,'.') #actual preview
#~ plt.subplot(2,2,3)
#~ plt.plot(tt,d_cc_x,'.') #actual preview
#~ plt.subplot(2,2,4)
#~ plt.plot(tt,d_cc_y,'.') #actual preview
log_dd_c_x.append(dd_c_x)
log_dd_c_y.append(dd_c_y)