pg = PgMini(Nstep,g,h,durrationOfStep,Dpy,beta_x,beta_y) pps=5 #point per step v=[1.0,0.1] p0=[-0.01,-0.01] x0=[[0,0] , [0,0]] comx=[] comy=[] LR=True plt.ion() t0=time.time() for k in range (80): #do 80 steps for ev in np.linspace(1.0/pps,1,pps): t=durrationOfStep*ev [c_x , c_y , d_c_x , d_c_y] = pg.computeNextCom(p0,x0,t) x=[[c_x,d_c_x] , [c_y,d_c_y]] if sigmaNoisePosition >0: x[0][0]+=np.random.normal(0,sigmaNoisePosition) #add some disturbance! x[1][0]+=np.random.normal(0,sigmaNoisePosition) if sigmaNoiseVelocity >0: x[0][1]+=np.random.normal(0,sigmaNoiseVelocity) x[1][1]+=np.random.normal(0,sigmaNoiseVelocity) comx.append(x[0][0]) comy.append(x[1][0]) if USE_WIIMOTE: v[0]=v[0]*0.2 + 0.8*(wm.state['acc'][0]-128)/50.0 v[1]=v[1]*0.2 + 0.8*(wm.state['acc'][1]-128)/50.0 if USE_GAMEPAD:
#!/usr/bin/env python #basic usage and benchmark: from minimal_pg import PgMini import matplotlib.pyplot as plt import numpy as np import time #initialisation of the pg pg = PgMini() #solve and return steps placement t0=time.time() #(tic tac mesurement) steps = pg.computeStepsPosition() print "compute time: " + str((time.time()-t0)*1e3) + " milliseconds" #get the COM preview [tt, cc_x , cc_y , d_cc_x , d_cc_y] = pg.computePreviewOfCom(steps) #get COM at a particular time value [c_x , c_y , d_c_x , d_c_y] = pg.computeNextCom(steps) #plot data plt.plot(cc_x,cc_y) plt.hold(True) plt.plot(steps[0],steps[1]) plt.plot(steps[0],steps[1]) plt.plot([c_x],[c_y],"D") plt.show()