#!/usr/bin/env python import matplotlib.pyplot as plt import numpy as np import time from minimal_pg_relaxed_zmp import PgMini # 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" # ~ for ery in np.linspace(-0.01,0.01,3): # ~ for erx in np.linspace(-0.01,0.01,3): # ~ x0err=[[0+erx,0+ery] , [0,0]] # ~ #get COM at a particular time value # ~ steps = pg.computeStepsPosition(alpha=0.0,p0=[-0.001,-0.005],v=[1.0,0.1],x0=x0err,LR=True) # ~ #get the COM preview # ~ [tt, cc_x , cc_y , d_cc_x , d_cc_y] = pg.computePreviewOfCom(steps,alpha=0.0,x0=x0err,N=20) # ~ plt.hold(True) # ~ plt.plot(steps[0],steps[1]) # ~ plt.plot(cc_x,cc_y) # ~ p0 = [0.0, 0.0] v = [1.0, 0.1]
N_COM_TO_DISPLAY = 10 #preview: number of point in a phase of COM (no impact on solution, display only)
USE_WIIMOTE=False
USE_GAMEPAD=True
DISPLAY_PREVIEW=True
ENABLE_LOGING=False
STOP_TIME = np.inf
sigmaNoisePosition=0.00 #optional noise on COM measurement
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]
cop=p0
lastFoot=[0.0102606,0.096]
def prepareCapsForStepPreviewInViewer (robot):
for i in range(Nstep):
if i == 0:
robot.viewer.gui.addSphere("world/pinocchio/capsSteps"+str(i),0.05,[1,0,0,0.5])
else:
