Esempio n. 1
0
#!/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]
Esempio n. 2
0
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: