x0 = [[0.0, 0.1], [0.0, 0.1]]
dt = pg.durrationOfStep / 30.0
N = pg.Nstep
LR = True
log_zmp_x = []
log_zmp_y = []
for k in range(5): # number of steps to do:
for i in range(30): # evolution in the current step
plt.figure(1)
plt.plot([p0[0]], [p0[1]], "dr")
plt.annotate("p0*", xy=(p0[0], p0[1]), xytext=(0, 0), textcoords="offset points")
plt.plot([x0[0][0]], [x0[1][0]], "Db")
steps = pg.computeStepsPosition(alpha, p0, v, x0, LR)
# ~ print p0[0]-steps[0][0]
# ~ print p0[1]-steps[1][0]
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),
log_vcomy_state=[]
log_dd_c_x=[]
log_dd_c_y=[]
log_t=[]
RUN_FLAG=True
ev=0.0
tk=0
while(RUN_FLAG):
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
