<jupyter_start><jupyter_text>MPC  Load simulated data<jupyter_code>def load():
    f = open(
        '/home/lars/crane_ws/src/crane/crane_controllers/data/datain.pickle', 'rb')
    datain = pickle.load(f)
    f.close()
    f = open(
        '/home/lars/crane_ws/src/crane/crane_controllers/data/dataout.pickle', 'rb')
    dataout = pickle.load(f)
    f.close()
    return datain, dataout
datain, dataout = load()

last_gopts = datain['last_gopt']
last_gs = datain['last_g']
Ls = datain['L']
ks = datain['k']
gammas = datain['gamma']
ts = datain['t']
zrefs = datain['zref']
zs = datain['z']

dot_wxs = dataout['dot_wx']
dot_wys = dataout['dot_wy']
gxs = dataout['gx']
gys = dataout['gy']
def cs_continuous_dynamics():
    kp = cs.SX.sym('kp')
    kd = cs.SX.sym('kd')
    k = cs.vertcat(kp, kd)
    
    L = cs.SX.sym('L')

    x0 = cs.SX.sym('x0')
    dx0 = cs.SX.sym('dx0')
    y0 = cs.SX.sym('y0')
    dy0 = cs.SX.sym('dy0')
    phix = cs.SX.sym('phix')
    dphix = cs.SX.sym('dphix')
    phiy = cs.SX.sym('phiy')
    dphiy = cs.SX.sym('dphiy')
    z = cs.vertcat(x0, dx0, y0, dy0, phix, dphix, phiy, dphiy)
    
    cx = cs.SX.cos(phix)
    sx = cs.SX.sin(phix)
    cy = cs.SX.cos(phiy)
    sy = cs.SX.sin(phiy)
    
    gx = cs.SX.sym('gx')
    gy = cs.SX.sym('gy')
    g = cs.vertcat(gx, gy)

    dzdt = cs.SX.sym('dzdt', 8,1)

    uy = - L*cy/cx*(kd*dphix + kp*phix) - 2.0*L*sy / \
        cx*dphix*dphiy + 9.81*sx*sy*sy/cx

    # Eq 51-54 in Ecc
    dzdt[0] = dx0  # dx0
    dzdt[1] = gx + (L*(kd*dphiy + kp*phiy) - L*cy*sy *
                    dphix*dphix - uy*sx*sy)/cy  # ddx0
    dzdt[2] = dy0  # dy0
    dzdt[3] = gy - (L*cy*(kd*dphix + kp*phix) + 2.0*L*sy *
                    dphix*dphiy - 9.81*sx*sy*sy)/cx  # ddy0
    dzdt[4] = z[5]  # dphix
    dzdt[5] = (-kd*dphix) - (kp*phix) - \
        (9.81/L*cy*sx) + (gy*cx/(cy*L))  # ddphix
    dzdt[6] = z[7]  # dphiy
    dzdt[7] = (-kd*dphiy) - (kp*phiy) - (9.81 / L * cx*sy) - \
        ((gx*cy + gy*sx*sy)/L)  # ddphiy
    
    f = cs.Function('cs', [z, g, k, L], [dzdt], ['z', 'g', 'k', 'L'], ['dzdt'])
    return f
def cs_discrete_dynamics():
    zk = cs.SX.sym('zk', 8)
    zk1 = cs.SX.sym('zk1', 8)
    gk = cs.SX.sym('gk', 2)
    Ts = cs.SX.sym('Ts')
    k = cs.SX.sym('k', 2)
    L = cs.SX.sym('L')
    
    cd = cs_continuous_dynamics()
    
    M = 10
    delta = Ts/M
    zk1 = zk
    for i in range(M):
        zk1 += delta * cd(zk1, gk, k, L)
    
    f = cs.Function('dd', [zk, gk, Ts, k, L], [zk1], ['zk', 'gk', 'Ts', 'k', 'L'], ['zk1'])
    return f
cd = cs_continuous_dynamics()
dd = cs_discrete_dynamics()
idx = 99
z = zs[idx]
g = last_gs[idx]
k = ks[idx]
L = Ls[idx]
Ts = 0.2
cd(z, g, k, L)
dd(z, g, Ts, k, L)
def cs_objective_function():
    
    z = cs.SX.sym('z', 8)
    zk = z
    zk1 = cs.SX.sym('zk1', 8)
    zref = cs.SX.sym('zref', 8)
    g = cs.SX.sym('g',4,2)
    last_g = cs.SX.sym('last_g',2)
    gk = cs.SX.sym('gk', 2)
    Ts = cs.SX.sym('Ts')
    k = cs.SX.sym('k', 2)
    L = cs.SX.sym('L')
    N = cs.SX.sym('N')
    
    R = cs.SX(np.diag([0.1, 0.1]))
    Q = cs.SX(np.diag([1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0]))
    
    discrete_dynamics = cs_discrete_dynamics()
    
    gmin = cs.SX(np.ones((4,2)) * -0.1)
    gmax = cs.SX(np.ones((4,2)) * 0.1)
    
    J = cs.SX(0)
    
    gk = g[0,:].T
    zk1 = discrete_dynamics(zk, gk, Ts, k, L)
    J = J + cs.mtimes([(zk1 - zref).T, Q, (zk1 - zref)]) + cs.mtimes([(gk - last_g).T, R, (gk - last_g)])
    zk = zk1
    
    gk = g[1,:].T
    zk1 = discrete_dynamics(zk, gk, Ts, k, L)
    J = J + cs.mtimes([(zk1 - zref).T, Q, (zk1 - zref)]) + cs.mtimes([(gk - g[0,:].T).T, R, (gk - g[0,:].T)])
    zk = zk1
    
    gk = g[2,:].T
    zk1 = discrete_dynamics(zk, gk, Ts, k, L)
    J = J + cs.mtimes([(zk1 - zref).T, Q, (zk1 - zref)]) + cs.mtimes([(gk - g[1,:].T).T, R, (gk - g[1,:].T)])
    zk = zk1
    
    gk = g[3,:].T
    zk1 = discrete_dynamics(zk, gk, Ts, k, L)
    J = J + cs.mtimes([(zk1 - zref).T, Q, (zk1 - zref)]) + cs.mtimes([(gk - g[2,:].T).T, R, (gk - g[2,:].T)])
    
    
    
    
    
    f = cs.Function('cost', [g, z, Ts, N, zref, k, last_g, L], [J], 
                    ['g', 'z', 'Ts', 'N', 'zref', 'k', 'last_g', 'L'], ['J'] )
    return f
        
    
obj = cs_objective_function()
idx = 1233
obj(last_gopts[:,:,idx], zs[idx], 0.2, 4, zrefs[idx], ks[idx], last_gs[idx], Ls[idx])
last_gopts[:,:,999]
zs[999]
obj
last_gopts[:,:,idx]
idx = 2499

t = ts[idx]
samptime = 0.6

last_g = cs.SX(last_gs[idx])

z = cs.SX(zs[idx])
zk = z
Ts = 0.2
N = 4
zref = cs.SX(zrefs[idx])
k = cs.SX(ks[idx])
last_g = cs.SX(last_gs[idx])
L = cs.SX(Ls[idx])

g = cs.SX.sym('g',8)

R = cs.SX(np.diag([0.1, 0.1]))
Q = cs.SX(np.diag([1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0]))

discrete_dynamics = cs_discrete_dynamics()

J = cs.SX(0)

gk = g[0:2]
zk1 = discrete_dynamics(zk, gk, Ts, k, L)
J = J + cs.mtimes([(zk1 - zref).T, Q, (zk1 - zref)]) + cs.mtimes([(gk - last_g).T, R, (gk - last_g)])
zk = zk1

gk = g[2:4]
zk1 = discrete_dynamics(zk, gk, Ts, k, L)
J = J + cs.mtimes([(zk1 - zref).T, Q, (zk1 - zref)]) + cs.mtimes([(gk - g[0:2]).T, R, (gk - g[0:2])])
zk = zk1

gk = g[4:6]
zk1 = discrete_dynamics(zk, gk, Ts, k, L)
J = J + cs.mtimes([(zk1 - zref).T, Q, (zk1 - zref)]) + cs.mtimes([(gk - g[2:4]).T, R, (gk - g[2:4])])
zk = zk1

gk = g[6:8]
zk1 = discrete_dynamics(zk, gk, Ts, k, L)
J = J + cs.mtimes([(zk1 - zref).T, Q, (zk1 - zref)]) + cs.mtimes([(gk - g[4:6]).T, R, (gk - g[4:6])])

nlp = {}                 # NLP declaration
nlp['x']= g              # decision vars
nlp['f'] = J             # objective
# nlp['g'] = g             # constraints

gmin = np.ones(8) * -0.1
gmax = np.ones(8) * 0.1


# Create solver instance
# F = cs.nlpsol('F','sqpmethod',nlp);
F = cs.nlpsol('F','ipopt',nlp);

#     F = cs.qpsol('F', 'qpoases', nlp, {'verbose': True})

# Solve the problem using a guess
sol = np.array(F(x0=last_gopts[:,:,idx].ravel(), lbx=gmin, ubx=gmax )['x']).reshape(-1,2)
print(sol)
sol
np.array([gxs[idx], gys[idx]])
plt.plot(gys)
last_gopts[:,:,idx].ravel()[0:2]
last_gopts[:,:,idx]<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.2423843240573028, 'fraction_numerical': 0.0363905812613206, 'mean_word_length': 2.420045045045045, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '1', 'dir': 'jupyter-structured-clean-dedup', 'id': '564898', 'license': 'MIT', 'n_tokens_mistral': 3233, 'n_tokens_neox': 3114, 'n_words': 600, 'path': 'crane_controllers/notebooks/mpc-sim.ipynb', 'repo_name': 'tingelst/crane', 'size': '6073'}