#[tt, cc_x , cc_y , d_cc_x , d_cc_y] = pg.computePreviewOfCom(steps,ev,x,N=N_COM_TO_DISPLAY)
#~ embed()
#~ showComPreviewInViewer(robot,[cc_x,cc_y])
[foot_x1,foot_y1]=[steps[0][1],steps[1][1]] #Goal for the flying foot (needed when t>ev_foot_const)
#~ [xf,dxf,ddxf , yf,dyf,ddyf , zf,dzf,ddzf , p1_star_x , p1_star_y]= ftg.get_next_foot( current_flying_foot[0],
#~ v_current_flying_foot[0],
#~ a_current_flying_foot[0],
#~
#~ current_flying_foot[1],
#~ v_current_flying_foot[1],
#~ a_current_flying_foot[1], foot_x1, foot_y1, t , durrationOfStep , dt)
[xf,dxf,ddxf , yf,dyf,ddyf , zf,dzf,ddzf , p1_star_x , p1_star_y]= ftg.get_next_foot( current_flying_foot[0],
v_current_flying_foot[0],
a_current_flying_foot[0],
current_flying_foot[1],
v_current_flying_foot[1],
a_current_flying_foot[1], foot_x1, foot_y1, t , td , dt)
p1_star=[p1_star_x,p1_star_y] #Realistic destination (=Goal if we have time... see "ev_foot_const")
if (FLAG_DOUBLE_SUPPORT):
[xf,dxf,ddxf]=[p1_star_x,0.0,0.0]
[yf,dyf,ddyf]=[p1_star_y,0.0,0.0]
[zf,dzf,ddzf]=[0.0,0.0,0.0]
#express foot acceleration as linear func of x1,y1
#~ ddxf=ftg.coeff_acc_x_lin_a * foot_x1 + ftg.coeff_acc_x_lin_b
#~ ddyf=ftg.coeff_acc_y_lin_a * foot_y1 + ftg.coeff_acc_y_lin_b
if LR :
gxx1=[x1]
gyy1=[y1]
x_int = x0
dx_int = dx0
ddx_int = ddx0
xx_int = [x_int]
dxx_int = [dx_int]
ddxx_int = [ddx_int]
while(t<0.99):
# f0+=(np.random.rand()-0.5)/100
# df0+=(np.random.rand()-0.5)/1000
# ddf0+=(np.random.rand()-0.5)/1000
[x0,dx0,ddx0 , y0,dy0,ddy0 , z0,dz0,ddz0 , gx1,gy1] = ftg.get_next_foot (x0, dx0, ddx0, y0, dy0, ddy0, x1, y1, t, 1.0 ,dt)
t+=dt
#itegration
ddx_int = ddx0
dx_int += ddx0*dt
x_int += dx_int*dt
#LOOP on integration:
x0= x_int
dx0= dx_int
ddx0=ddx_int
#~
xx_int.append(x_int)
