MPC = MPC_Formulation_DS(Nstep, g, h, durrationOfStep, Dpy, weighting_Delta_px, weighting_Delta_py, weighting_p0, weighting_p1, weighting_vel, weighting_cp, Tds, pps) p=PinocchioControllerAcceleration(dt,robot,q_init) ftg=Foot_trajectory_generator(fh,durrationOfStep * (1-ev_foot_const)) #initial feet positions initial_RF = np.array( robot.Mrf(q_init).translation ).flatten().tolist()[:2] initial_LF = np.array( robot.Mlf(q_init).translation ).flatten().tolist()[:2] p0_star = initial_RF#[0.0102606,-0.096] p0 = initial_RF#[0.0102606,-0.096] cop=p0 lastFoot= initial_LF#[0.0102606,0.096] #current foot position, speed and acceleration [foot_x0 ,foot_y0] =lastFoot [foot_dx0 ,foot_dy0] =[0.0,0.0] [foot_ddx0,foot_ddy0]=[0.0,0.0] current_flying_foot = [foot_x0 ,foot_y0 ] v_current_flying_foot = [foot_dx0 ,foot_dy0 ]
import numpy as np from IPython import embed import matplotlib.pyplot as plt from foot_trajectory_generator import Foot_trajectory_generator if (1==1): ftg = Foot_trajectory_generator(0.5,0.1) #testing x0=0.0 dx0=0.0 ddx0=0.0 y0=0.0 dy0=0.0 ddy0=0.0 z0=0.0 dz0=0.0 ddz0=0.0 t=0.0 dt=0.001 xx=[x0] yy=[y0] zz=[z0] dxx=[dx0] dyy=[dy0] dzz=[dz0] ddxx=[ddx0] ddyy=[ddy0]