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]
