#!/usr/bin/env python
# -*- coding: utf-8 -*-

import casadi as ca
import casadi.tools as ca_tools

import numpy as np
import time
from draw import Draw_MPC_tracking

def shift_movement(T, t0, x0, u, x_f, f):
    f_value = f(x0, u[:, 0])
    st = x0 + T*f_value.full()
    t = t0 + T
    u_end = np.concatenate((u[:, 1:], u[:, -1:]), axis=1)
    x_f = np.concatenate((x_f[:, 1:], x_f[:, -1:]), axis=1)

    return t, st, u_end, x_f

def desired_command_and_trajectory(t, T, x0_, N_):
    # initial state / last state
    x_ = x0_.reshape(1, -1).tolist()[0]
    u_ = []
    # states for the next N_ trajectories
    for i in range(N_):
        t_predict = t + T*i
        x_ref_ = 0.5 * t_predict
        y_ref_ = 1.0
        theta_ref_ = 0.0
        v_ref_ = 0.5
        omega_ref_ = 0.0
        if x_ref_ >= 12.0:
            x_ref_ = 12.0
            v_ref_ = 0.0
        x_.append(x_ref_)
        x_.append(y_ref_)
        x_.append(theta_ref_)
        u_.append(v_ref_)
        u_.append(omega_ref_)
    # return pose and command
    x_ = np.array(x_).reshape(N_+1, -1)
    u_ = np.array(u_).reshape(N, -1)
    return x_, u_

if __name__ == '__main__':
    T = 0.5 # sampling time [s]
    N = 8 # prediction horizon
    rob_diam = 0.3 # [m]
    v_max = 0.6
    omega_max = np.pi/4.0

    x = ca.SX.sym('x')
    y = ca.SX.sym('y')
    theta = ca.SX.sym('theta')
    states = ca.vertcat(x, y)
    states = ca.vertcat(states, theta)
    n_states = states.size()[0]

    v = ca.SX.sym('v')
    omega = ca.SX.sym('omega')
    controls = ca.vertcat(v, omega)
    n_controls = controls.size()[0]

    ## rhs
    rhs = ca.vertcat(v*ca.cos(theta), v*ca.sin(theta))
    rhs = ca.vertcat(rhs, omega)

    ## function
    f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs'])

    ## for MPC
    U = ca.SX.sym('U', n_controls, N)
    X = ca.SX.sym('X', n_states, N+1)
    U_ref = ca.SX.sym('U_ref', n_controls, N)
    X_ref = ca.SX.sym('X_ref', n_states, N+1)

    ### define
    Q = np.array([[1.0, 0.0, 0.0],[0.0, 5.0, 0.0],[0.0, 0.0, .1]])
    R = np.array([[0.5, 0.0], [0.0, 0.05]])
    #### cost function
    obj = 0 #### cost
    g = [] # equal constrains
    g.append(X[:, 0]-X_ref[:, 0])
    for i in range(N):
        state_error_ = X[:, i] - X_ref[:, i+1]
        control_error_ = U[:, i] - U_ref[:, i]
        obj = obj + ca.mtimes([state_error_.T, Q, state_error_]) + ca.mtimes([control_error_.T, R, control_error_])
        x_next_ = f(X[:, i], U[:, i])*T +X[:, i]
        g.append(X[:, i+1]-x_next_)

    opt_variables = ca.vertcat( ca.reshape(U, -1, 1), ca.reshape(X, -1, 1))
    opt_params = ca.vertcat(ca.reshape(U_ref, -1, 1), ca.reshape(X_ref, -1, 1))

    nlp_prob = {'f': obj, 'x': opt_variables, 'p':opt_params, 'g':ca.vertcat(*g)}
    opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}

    solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)

    lbg = 0.0
    ubg = 0.0
    lbx = []
    ubx = []
    for _ in range(N):
        lbx.append(-v_max)
        lbx.append(-omega_max)
        ubx.append(v_max)
        ubx.append(omega_max)
    for _ in range(N+1): # note that this is different with the method using structure
        lbx.append(-20.0)
        lbx.append(-2.0)
        lbx.append(-np.inf)
        ubx.append(20.0)
        ubx.append(2.0)
        ubx.append(np.inf)

    # Simulation
    t0 = 0.0
    init_state = np.array([0.0, 0.0, 0.0]).reshape(-1, 1)# initial state
    current_state = init_state.copy()
    u0 = np.array([0.0, 0.0]*N).reshape(-1, 2).T# np.ones((N, 2)) # controls
    next_trajectories = np.tile(current_state.reshape(1, -1), N+1).reshape(N+1, -1)
    next_states = next_trajectories.copy()
    next_controls = np.zeros((N, 2))
    x_c = [] # contains for the history of the state
    u_c = []
    t_c = [t0] # for the time
    xx = []
    sim_time = 30.0

    ## start MPC
    mpciter = 0
    start_time = time.time()
    index_t = []
    while(mpciter-sim_time/T<0.0):
        current_time = mpciter * T # current time
        ## set parameter
        c_p = np.concatenate((next_controls.reshape(-1, 1), next_trajectories.reshape(-1, 1)))
        # print('{0}'.format(next_states))
        # print('{0}'.format(next_states.T.reshape(-1, 1)[:6]))
        init_control = np.concatenate((u0.T.reshape(-1, 1), next_states.T.reshape(-1, 1)))
        t_ = time.time()
        res = solver(x0=init_control, p=c_p, lbg=lbg, lbx=lbx, ubg=ubg, ubx=ubx)
        index_t.append(time.time()- t_)
        estimated_opt = res['x'].full() # the feedback is in the series [u0, x0, u1, x1, ...]
        u0 = estimated_opt[:int(n_controls*N)].reshape(N, n_controls).T # (n_controls, N)
        x_m = estimated_opt[int(n_controls*N):].reshape(N+1, n_states).T# [n_states, N]
        x_c.append(x_m.T)
        u_c.append(u0[:, 0])
        t_c.append(t0)
        t0, current_state, u0, next_states = shift_movement(T, t0, current_state, u0, x_m, f)
        current_state = ca.reshape(current_state, -1, 1)
        current_state = current_state.full()
        xx.append(current_state)
        next_trajectories, next_controls = desired_command_and_trajectory(t0, T, current_state, N)
        mpciter = mpciter + 1
    t_v = np.array(index_t)
    print(t_v.mean())
    print((time.time() - start_time)/(mpciter))
    print(mpciter)
    draw_result = Draw_MPC_tracking(rob_diam=0.3, init_state=init_state, robot_states=xx )
02690759ac2f1f488b3df87e05e9e1340b8560c8
<mask token>


def shift_movement(T, t0, x0, u, x_f, f):
    f_value = f(x0, u[:, 0])
    st = x0 + T * f_value.full()
    t = t0 + T
    u_end = np.concatenate((u[:, 1:], u[:, -1:]), axis=1)
    x_f = np.concatenate((x_f[:, 1:], x_f[:, -1:]), axis=1)
    return t, st, u_end, x_f


<mask token>

<mask token>


def shift_movement(T, t0, x0, u, x_f, f):
    f_value = f(x0, u[:, 0])
    st = x0 + T * f_value.full()
    t = t0 + T
    u_end = np.concatenate((u[:, 1:], u[:, -1:]), axis=1)
    x_f = np.concatenate((x_f[:, 1:], x_f[:, -1:]), axis=1)
    return t, st, u_end, x_f


def desired_command_and_trajectory(t, T, x0_, N_):
    x_ = x0_.reshape(1, -1).tolist()[0]
    u_ = []
    for i in range(N_):
        t_predict = t + T * i
        x_ref_ = 0.5 * t_predict
        y_ref_ = 1.0
        theta_ref_ = 0.0
        v_ref_ = 0.5
        omega_ref_ = 0.0
        if x_ref_ >= 12.0:
            x_ref_ = 12.0
            v_ref_ = 0.0
        x_.append(x_ref_)
        x_.append(y_ref_)
        x_.append(theta_ref_)
        u_.append(v_ref_)
        u_.append(omega_ref_)
    x_ = np.array(x_).reshape(N_ + 1, -1)
    u_ = np.array(u_).reshape(N, -1)
    return x_, u_


<mask token>

<mask token>


def shift_movement(T, t0, x0, u, x_f, f):
    f_value = f(x0, u[:, 0])
    st = x0 + T * f_value.full()
    t = t0 + T
    u_end = np.concatenate((u[:, 1:], u[:, -1:]), axis=1)
    x_f = np.concatenate((x_f[:, 1:], x_f[:, -1:]), axis=1)
    return t, st, u_end, x_f


def desired_command_and_trajectory(t, T, x0_, N_):
    x_ = x0_.reshape(1, -1).tolist()[0]
    u_ = []
    for i in range(N_):
        t_predict = t + T * i
        x_ref_ = 0.5 * t_predict
        y_ref_ = 1.0
        theta_ref_ = 0.0
        v_ref_ = 0.5
        omega_ref_ = 0.0
        if x_ref_ >= 12.0:
            x_ref_ = 12.0
            v_ref_ = 0.0
        x_.append(x_ref_)
        x_.append(y_ref_)
        x_.append(theta_ref_)
        u_.append(v_ref_)
        u_.append(omega_ref_)
    x_ = np.array(x_).reshape(N_ + 1, -1)
    u_ = np.array(u_).reshape(N, -1)
    return x_, u_


if __name__ == '__main__':
    T = 0.5
    N = 8
    rob_diam = 0.3
    v_max = 0.6
    omega_max = np.pi / 4.0
    x = ca.SX.sym('x')
    y = ca.SX.sym('y')
    theta = ca.SX.sym('theta')
    states = ca.vertcat(x, y)
    states = ca.vertcat(states, theta)
    n_states = states.size()[0]
    v = ca.SX.sym('v')
    omega = ca.SX.sym('omega')
    controls = ca.vertcat(v, omega)
    n_controls = controls.size()[0]
    rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta))
    rhs = ca.vertcat(rhs, omega)
    f = ca.Function('f', [states, controls], [rhs], ['input_state',
        'control_input'], ['rhs'])
    U = ca.SX.sym('U', n_controls, N)
    X = ca.SX.sym('X', n_states, N + 1)
    U_ref = ca.SX.sym('U_ref', n_controls, N)
    X_ref = ca.SX.sym('X_ref', n_states, N + 1)
    Q = np.array([[1.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, 0.1]])
    R = np.array([[0.5, 0.0], [0.0, 0.05]])
    obj = 0
    g = []
    g.append(X[:, 0] - X_ref[:, 0])
    for i in range(N):
        state_error_ = X[:, i] - X_ref[:, i + 1]
        control_error_ = U[:, i] - U_ref[:, i]
        obj = obj + ca.mtimes([state_error_.T, Q, state_error_]) + ca.mtimes([
            control_error_.T, R, control_error_])
        x_next_ = f(X[:, i], U[:, i]) * T + X[:, i]
        g.append(X[:, i + 1] - x_next_)
    opt_variables = ca.vertcat(ca.reshape(U, -1, 1), ca.reshape(X, -1, 1))
    opt_params = ca.vertcat(ca.reshape(U_ref, -1, 1), ca.reshape(X_ref, -1, 1))
    nlp_prob = {'f': obj, 'x': opt_variables, 'p': opt_params, 'g': ca.
        vertcat(*g)}
    opts_setting = {'ipopt.max_iter': 100, 'ipopt.print_level': 0,
        'print_time': 0, 'ipopt.acceptable_tol': 1e-08,
        'ipopt.acceptable_obj_change_tol': 1e-06}
    solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
    lbg = 0.0
    ubg = 0.0
    lbx = []
    ubx = []
    for _ in range(N):
        lbx.append(-v_max)
        lbx.append(-omega_max)
        ubx.append(v_max)
        ubx.append(omega_max)
    for _ in range(N + 1):
        lbx.append(-20.0)
        lbx.append(-2.0)
        lbx.append(-np.inf)
        ubx.append(20.0)
        ubx.append(2.0)
        ubx.append(np.inf)
    t0 = 0.0
    init_state = np.array([0.0, 0.0, 0.0]).reshape(-1, 1)
    current_state = init_state.copy()
    u0 = np.array([0.0, 0.0] * N).reshape(-1, 2).T
    next_trajectories = np.tile(current_state.reshape(1, -1), N + 1).reshape(
        N + 1, -1)
    next_states = next_trajectories.copy()
    next_controls = np.zeros((N, 2))
    x_c = []
    u_c = []
    t_c = [t0]
    xx = []
    sim_time = 30.0
    mpciter = 0
    start_time = time.time()
    index_t = []
    while mpciter - sim_time / T < 0.0:
        current_time = mpciter * T
        c_p = np.concatenate((next_controls.reshape(-1, 1),
            next_trajectories.reshape(-1, 1)))
        init_control = np.concatenate((u0.T.reshape(-1, 1), next_states.T.
            reshape(-1, 1)))
        t_ = time.time()
        res = solver(x0=init_control, p=c_p, lbg=lbg, lbx=lbx, ubg=ubg, ubx=ubx
            )
        index_t.append(time.time() - t_)
        estimated_opt = res['x'].full()
        u0 = estimated_opt[:int(n_controls * N)].reshape(N, n_controls).T
        x_m = estimated_opt[int(n_controls * N):].reshape(N + 1, n_states).T
        x_c.append(x_m.T)
        u_c.append(u0[:, 0])
        t_c.append(t0)
        t0, current_state, u0, next_states = shift_movement(T, t0,
            current_state, u0, x_m, f)
        current_state = ca.reshape(current_state, -1, 1)
        current_state = current_state.full()
        xx.append(current_state)
        next_trajectories, next_controls = desired_command_and_trajectory(t0,
            T, current_state, N)
        mpciter = mpciter + 1
    t_v = np.array(index_t)
    print(t_v.mean())
    print((time.time() - start_time) / mpciter)
    print(mpciter)
    draw_result = Draw_MPC_tracking(rob_diam=0.3, init_state=init_state,
        robot_states=xx)

import casadi as ca
import casadi.tools as ca_tools
import numpy as np
import time
from draw import Draw_MPC_tracking


def shift_movement(T, t0, x0, u, x_f, f):
    f_value = f(x0, u[:, 0])
    st = x0 + T * f_value.full()
    t = t0 + T
    u_end = np.concatenate((u[:, 1:], u[:, -1:]), axis=1)
    x_f = np.concatenate((x_f[:, 1:], x_f[:, -1:]), axis=1)
    return t, st, u_end, x_f


def desired_command_and_trajectory(t, T, x0_, N_):
    x_ = x0_.reshape(1, -1).tolist()[0]
    u_ = []
    for i in range(N_):
        t_predict = t + T * i
        x_ref_ = 0.5 * t_predict
        y_ref_ = 1.0
        theta_ref_ = 0.0
        v_ref_ = 0.5
        omega_ref_ = 0.0
        if x_ref_ >= 12.0:
            x_ref_ = 12.0
            v_ref_ = 0.0
        x_.append(x_ref_)
        x_.append(y_ref_)
        x_.append(theta_ref_)
        u_.append(v_ref_)
        u_.append(omega_ref_)
    x_ = np.array(x_).reshape(N_ + 1, -1)
    u_ = np.array(u_).reshape(N, -1)
    return x_, u_


if __name__ == '__main__':
    T = 0.5
    N = 8
    rob_diam = 0.3
    v_max = 0.6
    omega_max = np.pi / 4.0
    x = ca.SX.sym('x')
    y = ca.SX.sym('y')
    theta = ca.SX.sym('theta')
    states = ca.vertcat(x, y)
    states = ca.vertcat(states, theta)
    n_states = states.size()[0]
    v = ca.SX.sym('v')
    omega = ca.SX.sym('omega')
    controls = ca.vertcat(v, omega)
    n_controls = controls.size()[0]
    rhs = ca.vertcat(v * ca.cos(theta), v * ca.sin(theta))
    rhs = ca.vertcat(rhs, omega)
    f = ca.Function('f', [states, controls], [rhs], ['input_state',
        'control_input'], ['rhs'])
    U = ca.SX.sym('U', n_controls, N)
    X = ca.SX.sym('X', n_states, N + 1)
    U_ref = ca.SX.sym('U_ref', n_controls, N)
    X_ref = ca.SX.sym('X_ref', n_states, N + 1)
    Q = np.array([[1.0, 0.0, 0.0], [0.0, 5.0, 0.0], [0.0, 0.0, 0.1]])
    R = np.array([[0.5, 0.0], [0.0, 0.05]])
    obj = 0
    g = []
    g.append(X[:, 0] - X_ref[:, 0])
    for i in range(N):
        state_error_ = X[:, i] - X_ref[:, i + 1]
        control_error_ = U[:, i] - U_ref[:, i]
        obj = obj + ca.mtimes([state_error_.T, Q, state_error_]) + ca.mtimes([
            control_error_.T, R, control_error_])
        x_next_ = f(X[:, i], U[:, i]) * T + X[:, i]
        g.append(X[:, i + 1] - x_next_)
    opt_variables = ca.vertcat(ca.reshape(U, -1, 1), ca.reshape(X, -1, 1))
    opt_params = ca.vertcat(ca.reshape(U_ref, -1, 1), ca.reshape(X_ref, -1, 1))
    nlp_prob = {'f': obj, 'x': opt_variables, 'p': opt_params, 'g': ca.
        vertcat(*g)}
    opts_setting = {'ipopt.max_iter': 100, 'ipopt.print_level': 0,
        'print_time': 0, 'ipopt.acceptable_tol': 1e-08,
        'ipopt.acceptable_obj_change_tol': 1e-06}
    solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
    lbg = 0.0
    ubg = 0.0
    lbx = []
    ubx = []
    for _ in range(N):
        lbx.append(-v_max)
        lbx.append(-omega_max)
        ubx.append(v_max)
        ubx.append(omega_max)
    for _ in range(N + 1):
        lbx.append(-20.0)
        lbx.append(-2.0)
        lbx.append(-np.inf)
        ubx.append(20.0)
        ubx.append(2.0)
        ubx.append(np.inf)
    t0 = 0.0
    init_state = np.array([0.0, 0.0, 0.0]).reshape(-1, 1)
    current_state = init_state.copy()
    u0 = np.array([0.0, 0.0] * N).reshape(-1, 2).T
    next_trajectories = np.tile(current_state.reshape(1, -1), N + 1).reshape(
        N + 1, -1)
    next_states = next_trajectories.copy()
    next_controls = np.zeros((N, 2))
    x_c = []
    u_c = []
    t_c = [t0]
    xx = []
    sim_time = 30.0
    mpciter = 0
    start_time = time.time()
    index_t = []
    while mpciter - sim_time / T < 0.0:
        current_time = mpciter * T
        c_p = np.concatenate((next_controls.reshape(-1, 1),
            next_trajectories.reshape(-1, 1)))
        init_control = np.concatenate((u0.T.reshape(-1, 1), next_states.T.
            reshape(-1, 1)))
        t_ = time.time()
        res = solver(x0=init_control, p=c_p, lbg=lbg, lbx=lbx, ubg=ubg, ubx=ubx
            )
        index_t.append(time.time() - t_)
        estimated_opt = res['x'].full()
        u0 = estimated_opt[:int(n_controls * N)].reshape(N, n_controls).T
        x_m = estimated_opt[int(n_controls * N):].reshape(N + 1, n_states).T
        x_c.append(x_m.T)
        u_c.append(u0[:, 0])
        t_c.append(t0)
        t0, current_state, u0, next_states = shift_movement(T, t0,
            current_state, u0, x_m, f)
        current_state = ca.reshape(current_state, -1, 1)
        current_state = current_state.full()
        xx.append(current_state)
        next_trajectories, next_controls = desired_command_and_trajectory(t0,
            T, current_state, N)
        mpciter = mpciter + 1
    t_v = np.array(index_t)
    print(t_v.mean())
    print((time.time() - start_time) / mpciter)
    print(mpciter)
    draw_result = Draw_MPC_tracking(rob_diam=0.3, init_state=init_state,
        robot_states=xx)

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import casadi as ca
import casadi.tools as ca_tools

import numpy as np
import time
from draw import Draw_MPC_tracking

def shift_movement(T, t0, x0, u, x_f, f):
    f_value = f(x0, u[:, 0])
    st = x0 + T*f_value.full()
    t = t0 + T
    u_end = np.concatenate((u[:, 1:], u[:, -1:]), axis=1)
    x_f = np.concatenate((x_f[:, 1:], x_f[:, -1:]), axis=1)

    return t, st, u_end, x_f

def desired_command_and_trajectory(t, T, x0_, N_):
    # initial state / last state
    x_ = x0_.reshape(1, -1).tolist()[0]
    u_ = []
    # states for the next N_ trajectories
    for i in range(N_):
        t_predict = t + T*i
        x_ref_ = 0.5 * t_predict
        y_ref_ = 1.0
        theta_ref_ = 0.0
        v_ref_ = 0.5
        omega_ref_ = 0.0
        if x_ref_ >= 12.0:
            x_ref_ = 12.0
            v_ref_ = 0.0
        x_.append(x_ref_)
        x_.append(y_ref_)
        x_.append(theta_ref_)
        u_.append(v_ref_)
        u_.append(omega_ref_)
    # return pose and command
    x_ = np.array(x_).reshape(N_+1, -1)
    u_ = np.array(u_).reshape(N, -1)
    return x_, u_

if __name__ == '__main__':
    T = 0.5 # sampling time [s]
    N = 8 # prediction horizon
    rob_diam = 0.3 # [m]
    v_max = 0.6
    omega_max = np.pi/4.0

    x = ca.SX.sym('x')
    y = ca.SX.sym('y')
    theta = ca.SX.sym('theta')
    states = ca.vertcat(x, y)
    states = ca.vertcat(states, theta)
    n_states = states.size()[0]

    v = ca.SX.sym('v')
    omega = ca.SX.sym('omega')
    controls = ca.vertcat(v, omega)
    n_controls = controls.size()[0]

    ## rhs
    rhs = ca.vertcat(v*ca.cos(theta), v*ca.sin(theta))
    rhs = ca.vertcat(rhs, omega)

    ## function
    f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs'])

    ## for MPC
    U = ca.SX.sym('U', n_controls, N)
    X = ca.SX.sym('X', n_states, N+1)
    U_ref = ca.SX.sym('U_ref', n_controls, N)
    X_ref = ca.SX.sym('X_ref', n_states, N+1)

    ### define
    Q = np.array([[1.0, 0.0, 0.0],[0.0, 5.0, 0.0],[0.0, 0.0, .1]])
    R = np.array([[0.5, 0.0], [0.0, 0.05]])
    #### cost function
    obj = 0 #### cost
    g = [] # equal constrains
    g.append(X[:, 0]-X_ref[:, 0])
    for i in range(N):
        state_error_ = X[:, i] - X_ref[:, i+1]
        control_error_ = U[:, i] - U_ref[:, i]
        obj = obj + ca.mtimes([state_error_.T, Q, state_error_]) + ca.mtimes([control_error_.T, R, control_error_])
        x_next_ = f(X[:, i], U[:, i])*T +X[:, i]
        g.append(X[:, i+1]-x_next_)

    opt_variables = ca.vertcat( ca.reshape(U, -1, 1), ca.reshape(X, -1, 1))
    opt_params = ca.vertcat(ca.reshape(U_ref, -1, 1), ca.reshape(X_ref, -1, 1))

    nlp_prob = {'f': obj, 'x': opt_variables, 'p':opt_params, 'g':ca.vertcat(*g)}
    opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}

    solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)

    lbg = 0.0
    ubg = 0.0
    lbx = []
    ubx = []
    for _ in range(N):
        lbx.append(-v_max)
        lbx.append(-omega_max)
        ubx.append(v_max)
        ubx.append(omega_max)
    for _ in range(N+1): # note that this is different with the method using structure
        lbx.append(-20.0)
        lbx.append(-2.0)
        lbx.append(-np.inf)
        ubx.append(20.0)
        ubx.append(2.0)
        ubx.append(np.inf)

    # Simulation
    t0 = 0.0
    init_state = np.array([0.0, 0.0, 0.0]).reshape(-1, 1)# initial state
    current_state = init_state.copy()
    u0 = np.array([0.0, 0.0]*N).reshape(-1, 2).T# np.ones((N, 2)) # controls
    next_trajectories = np.tile(current_state.reshape(1, -1), N+1).reshape(N+1, -1)
    next_states = next_trajectories.copy()
    next_controls = np.zeros((N, 2))
    x_c = [] # contains for the history of the state
    u_c = []
    t_c = [t0] # for the time
    xx = []
    sim_time = 30.0

    ## start MPC
    mpciter = 0
    start_time = time.time()
    index_t = []
    while(mpciter-sim_time/T<0.0):
        current_time = mpciter * T # current time
        ## set parameter
        c_p = np.concatenate((next_controls.reshape(-1, 1), next_trajectories.reshape(-1, 1)))
        # print('{0}'.format(next_states))
        # print('{0}'.format(next_states.T.reshape(-1, 1)[:6]))
        init_control = np.concatenate((u0.T.reshape(-1, 1), next_states.T.reshape(-1, 1)))
        t_ = time.time()
        res = solver(x0=init_control, p=c_p, lbg=lbg, lbx=lbx, ubg=ubg, ubx=ubx)
        index_t.append(time.time()- t_)
        estimated_opt = res['x'].full() # the feedback is in the series [u0, x0, u1, x1, ...]
        u0 = estimated_opt[:int(n_controls*N)].reshape(N, n_controls).T # (n_controls, N)
        x_m = estimated_opt[int(n_controls*N):].reshape(N+1, n_states).T# [n_states, N]
        x_c.append(x_m.T)
        u_c.append(u0[:, 0])
        t_c.append(t0)
        t0, current_state, u0, next_states = shift_movement(T, t0, current_state, u0, x_m, f)
        current_state = ca.reshape(current_state, -1, 1)
        current_state = current_state.full()
        xx.append(current_state)
        next_trajectories, next_controls = desired_command_and_trajectory(t0, T, current_state, N)
        mpciter = mpciter + 1
    t_v = np.array(index_t)
    print(t_v.mean())
    print((time.time() - start_time)/(mpciter))
    print(mpciter)
    draw_result = Draw_MPC_tracking(rob_diam=0.3, init_state=init_state, robot_states=xx )