#[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)