#!/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()
pygame.init()
pygame.joystick.init()
pygame.event.pump()
if (pygame.joystick.get_count() > 0):
print "found gamepad! : " + pygame.joystick.Joystick(0).get_name()
my_joystick = pygame.joystick.Joystick(0)
my_joystick.init()
else :
print "No gamepad found"
USE_GAMEPAD = False
sigmaNoisePosition=0.0
sigmaNoiseVelocity=0.00
#initialisation of the pg
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)
beta_y=8.0
N_COM_TO_DISPLAY = 10
USE_WIIMOTE=False
USE_GAMEPAD=True
DISPLAY_PREVIEW=True
STOP_TIME = np.inf
sigmaNoisePosition=0.00
sigmaNoiseVelocity=0.00
#initialisation of the pg
dt=durrationOfStep/pps
print( "dt= "+str(dt*1000)+"ms")
pg = PgMini(Nstep,g,h,durrationOfStep,Dpy,beta_x,beta_y)
p=PinocchioControllerAcceleration(dt)
v=[1.0,1.0]
#initial feet positions
p0 =[0.0102606,-0.096]
lastFoot=[0.0102606,0.096]
#p0=[0.0102606, -0.01]
#0.0102606 -0.096 0.0669995
#~ #0.0102606 0.096 0.0669995
def prepareCapsForStepPreviewInViewer (robot):
for i in range(Nstep):
robot.viewer.gui.addSphere("world/pinocchio/capsSteps"+str(i),0.05,[0,1,0,0.5])
