#!/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()
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:
pygame.event.pump()
v[0]=my_joystick.get_axis(0)
v[1]=-my_joystick.get_axis(1)
print x
steps = pg.computeStepsPosition(ev,p0,v,x,LR)
[tt, cc_x , cc_y , d_cc_x , d_cc_y] = pg.computePreviewOfCom(steps,ev,x,N=20)
#plot data
plt.axis((-1,5,-1,1))
plt.plot(cc_x,cc_y,'g',lw=0.5)
plt.hold(True)
plt.plot(steps[0],steps[1],'rD')
plt.plot([steps[0][0]],[steps[1][0]],'bD')
plt.plot([c_x],[c_y],"D")
plt.plot(comx,comy,"k")
plt.draw()
FlagRT = False
while(time.time()-t0 < (durrationOfStep/pps)):
FlagRT = True
if not FlagRT :
if c_x<-1.0:
v[0]=1.0
if c_y>1.0:
v[1]=-1.0
if c_y<-1.0:
v[1]=1.0
steps = pg.computeStepsPosition(ev,p0,v,x,LR)
cop=[steps[0][0],steps[1][0]]
showStepPreviewInViewer(p.robot,steps)
currentFoot = [steps[0][0],steps[1][0]]
nextFoot = [steps[0][1],steps[1][1]]
if DISPLAY_PREVIEW:
[tt, cc_x , cc_y , d_cc_x , d_cc_y] = pg.computePreviewOfCom(steps,ev,x,N=N_COM_TO_DISPLAY)
showComPreviewInViewer(p.robot,[cc_x,cc_y])
#plot data
#plt.axis((-1,5,-1,1))
#plt.plot(cc_x,cc_y,'g',lw=0.5)
#plt.hold(True)
#plt.plot(steps[0],steps[1],'rD')
#plt.plot([steps[0][0]],[steps[1][0]],'bD')
#plt.plot([c_x],[c_y],"D")
#plt.plot(comx,comy,"k")
#plt.plot([steps[0][1]],[steps[1][1]],"Dy")
[xf,yf,zf,dxf,dyf,dzf] = foot_interpolate(lastFoot[0],lastFoot[1],nextFoot[0],nextFoot[1],ev,durrationOfStep)
#plt.plot([xf],[yf],"Dg")
