
###Creating the model
import numpy as np

# Add do_mpc to path. This is not necessary if it was installed via pip.
import sys
sys.path.append('../../')

# Import do_mpc package:
import do_mpc
model_type = 'continuous' # either 'discrete' or 'continuous'

##Model variables
model = do_mpc.model.Model(model_type)

#phi_1 = model.set_variable(var_type='_x', var_name='phi_1', shape=(1,1))
#phi_2 = model.set_variable(var_type='_x', var_name='phi_2', shape=(1,1))
#phi_3 = model.set_variable(var_type='_x', var_name='phi_3', shape=(1,1))
PV_FC_25 = model.set_variable(var_type='_x', var_name='PV_FC_25', shape=(1,1))
L_max = model.set_variable(var_type='_x', var_name='L_max', shape=(1,1)) 
L_min = model.set_variable(var_type='_x', var_name='L_min', shape=(1,1))
PV_MV_25 = model.set_variable(var_type='_x', var_name='MV_FC_25', shape=(1,1))
PV_FC_10 = model.set_variable(var_type='_x', var_name='PV_FC_10', shape=(1,1))


# Variables can also be vectors:
dphi = model.set_variable(var_type='_x', var_name='dphi', shape=(3,1))
# Two states for the desired (set) motor position:
phi_m_1_set = model.set_variable(var_type='_u', var_name='phi_m_1_set')
phi_m_2_set = model.set_variable(var_type='_u', var_name='phi_m_2_set')
# Two additional states for the true motor position:
phi_1_m = model.set_variable(var_type='_x', var_name='phi_1_m', shape=(1,1))
phi_2_m = model.set_variable(var_type='_x', var_name='phi_2_m', shape=(1,1))

print('phi_1={}, with phi_1.shape={}'.format(phi_1, phi_1.shape))
print('dphi={}, with dphi.shape={}'.format(dphi, dphi.shape))

##Query variable

model.x

model.x['phi_1']
bool(model.x['phi_1'] == phi_1)

model.x['dphi',0]

model.x.labels()

##Model parameters

# As shown in the table above, we can use Long names or short names for the variable type.
Theta_1 = model.set_variable('parameter', 'Theta_1')
Theta_2 = model.set_variable('parameter', 'Theta_2')
Theta_3 = model.set_variable('parameter', 'Theta_3')

# 'C' is spring constant and 'd' is damping constant of three disks

c = np.array([2.697,  2.66,  3.05, 2.86])*1e-3
d = np.array([6.78,  8.01,  8.82])*1e-5


##Right-hand-side equation

model.set_rhs('phi_1', dphi[0])
model.set_rhs('phi_2', dphi[1])
model.set_rhs('phi_3', dphi[2])

from casadi import *

dphi_next = vertcat(
    -c[0]/Theta_1*(phi_1-phi_1_m)-c[1]/Theta_1*(phi_1-phi_2)-d[0]/Theta_1*dphi[0],
    -c[1]/Theta_2*(phi_2-phi_1)-c[2]/Theta_2*(phi_2-phi_3)-d[1]/Theta_2*dphi[1],
    -c[2]/Theta_3*(phi_3-phi_2)-c[3]/Theta_3*(phi_3-phi_2_m)-d[2]/Theta_3*dphi[2],
)

model.set_rhs('dphi', dphi_next)

tau = 1e-2
model.set_rhs('phi_1_m', 1/tau*(phi_m_1_set - phi_1_m))
model.set_rhs('phi_2_m', 1/tau*(phi_m_2_set - phi_2_m))
model.setup()

###Configurating MPC controller

mpc = do_mpc.controller.MPC(model)

##Optimizer parameters

setup_mpc = {
    'n_horizon': 20,
    't_step': 0.1,
    'n_robust': 1,
    'store_full_solution': True,
}
mpc.set_param(**setup_mpc)


##Objective function

mterm = phi_1**2 + phi_2**2 + phi_3**2
lterm = phi_1**2 + phi_2**2 + phi_3**2

mpc.set_objective(mterm=mterm, lterm=lterm)

mpc.set_rterm(
    phi_m_1_set=1e-2,
    phi_m_2_set=1e-2
)

##Constrains

# Lower bounds on states:
mpc.bounds['lower','_x', 'phi_1'] = -2*np.pi
mpc.bounds['lower','_x', 'phi_2'] = -2*np.pi
mpc.bounds['lower','_x', 'phi_3'] = -2*np.pi
# Upper bounds on states
mpc.bounds['upper','_x', 'phi_1'] = 2*np.pi
mpc.bounds['upper','_x', 'phi_2'] = 2*np.pi
mpc.bounds['upper','_x', 'phi_3'] = 2*np.pi

# Lower bounds on inputs:
mpc.bounds['lower','_u', 'phi_m_1_set'] = -2*np.pi
mpc.bounds['lower','_u', 'phi_m_2_set'] = -2*np.pi
# Lower bounds on inputs:
mpc.bounds['upper','_u', 'phi_m_1_set'] = 2*np.pi
mpc.bounds['upper','_u', 'phi_m_2_set'] = 2*np.pi


##Scaling 


mpc.scaling['_x', 'phi_1'] = 2
mpc.scaling['_x', 'phi_2'] = 2
mpc.scaling['_x', 'phi_3'] = 2


##Uncertain parameters


inertia_mass_1 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_2 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_3 = 2.25*1e-4*np.array([1.])

mpc.set_uncertainty_values(
    Theta_1 = inertia_mass_1,
    Theta_2 = inertia_mass_2,
    Theta_3 = inertia_mass_3
)


inertia_mass_1 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_2 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_3 = 2.25*1e-4*np.array([1.])

mpc.set_uncertainty_values(
    Theta_1 = inertia_mass_1,
    Theta_2 = inertia_mass_2,
    Theta_3 = inertia_mass_3
)

##Setup

mpc.setup()

### Configurating the simulator

simulator = do_mpc.simulator.Simulator(model)
# Instead of supplying a dict with the splat operator (**), as with the optimizer.set_param(),
# we can also use keywords (and call the method multiple times, if necessary):

##Simulator parameters

simulator.set_param(t_step = 0.1)

##Uncertain parameters

p_template = simulator.get_p_template()
type(p_template)
p_template.keys()
def p_fun(t_now):
    p_template['Theta_1'] = 2.25e-4
    p_template['Theta_2'] = 2.25e-4
    p_template['Theta_3'] = 2.25e-4
    return p_template
    
simulator.set_p_fun(p_fun)



## Running the optimizer   
u0 = mpc.make_step(x0)



###Creating the model
import numpy as np

# Add do_mpc to path. This is not necessary if it was installed via pip.
import sys
sys.path.append('../../')

# Import do_mpc package:
import do_mpc
model_type = 'continuous' # either 'discrete' or 'continuous'

##Model variables
model = do_mpc.model.Model(model_type)

#phi_1 = model.set_variable(var_type='_x', var_name='phi_1', shape=(1,1))
#phi_2 = model.set_variable(var_type='_x', var_name='phi_2', shape=(1,1))
#phi_3 = model.set_variable(var_type='_x', var_name='phi_3', shape=(1,1))
PV_FC_25 = model.set_variable(var_type='_x', var_name='PV_FC_25', shape=(1,1))
L_max = model.set_variable(var_type='_x', var_name='L_max', shape=(1,1)) 
L_min = model.set_variable(var_type='_x', var_name='L_min', shape=(1,1))
PV_MV_25 = model.set_variable(var_type='_x', var_name='MV_FC_25', shape=(1,1))
PV_FC_10 = model.set_variable(var_type='_x', var_name='PV_FC_10', shape=(1,1))


# Variables can also be vectors:
dphi = model.set_variable(var_type='_x', var_name='dphi', shape=(3,1))
# Two states for the desired (set) motor position:
phi_m_1_set = model.set_variable(var_type='_u', var_name='phi_m_1_set')
phi_m_2_set = model.set_variable(var_type='_u', var_name='phi_m_2_set')
# Two additional states for the true motor position:
phi_1_m = model.set_variable(var_type='_x', var_name='phi_1_m', shape=(1,1))
phi_2_m = model.set_variable(var_type='_x', var_name='phi_2_m', shape=(1,1))

print('phi_1={}, with phi_1.shape={}'.format(phi_1, phi_1.shape))
print('dphi={}, with dphi.shape={}'.format(dphi, dphi.shape))

##Query variable

model.x

model.x['phi_1']
bool(model.x['phi_1'] == phi_1)

model.x['dphi',0]

model.x.labels()

##Model parameters

# As shown in the table above, we can use Long names or short names for the variable type.
Theta_1 = model.set_variable('parameter', 'Theta_1')
Theta_2 = model.set_variable('parameter', 'Theta_2')
Theta_3 = model.set_variable('parameter', 'Theta_3')

# 'C' is spring constant and 'd' is damping constant of three disks

c = np.array([2.697,  2.66,  3.05, 2.86])*1e-3
d = np.array([6.78,  8.01,  8.82])*1e-5


##Right-hand-side equation

model.set_rhs('phi_1', dphi[0])
model.set_rhs('phi_2', dphi[1])
model.set_rhs('phi_3', dphi[2])

from casadi import *

dphi_next = vertcat(
    -c[0]/Theta_1*(phi_1-phi_1_m)-c[1]/Theta_1*(phi_1-phi_2)-d[0]/Theta_1*dphi[0],
    -c[1]/Theta_2*(phi_2-phi_1)-c[2]/Theta_2*(phi_2-phi_3)-d[1]/Theta_2*dphi[1],
    -c[2]/Theta_3*(phi_3-phi_2)-c[3]/Theta_3*(phi_3-phi_2_m)-d[2]/Theta_3*dphi[2],
)

model.set_rhs('dphi', dphi_next)

tau = 1e-2
model.set_rhs('phi_1_m', 1/tau*(phi_m_1_set - phi_1_m))
model.set_rhs('phi_2_m', 1/tau*(phi_m_2_set - phi_2_m))
model.setup()

###Configurating MPC controller

mpc = do_mpc.controller.MPC(model)

##Optimizer parameters

setup_mpc = {
    'n_horizon': 20,
    't_step': 0.1,
    'n_robust': 1,
    'store_full_solution': True,
}
mpc.set_param(**setup_mpc)


##Objective function

mterm = phi_1**2 + phi_2**2 + phi_3**2
lterm = phi_1**2 + phi_2**2 + phi_3**2

mpc.set_objective(mterm=mterm, lterm=lterm)

mpc.set_rterm(
    phi_m_1_set=1e-2,
    phi_m_2_set=1e-2
)

##Constrains

# Lower bounds on states:
mpc.bounds['lower','_x', 'phi_1'] = -2*np.pi
mpc.bounds['lower','_x', 'phi_2'] = -2*np.pi
mpc.bounds['lower','_x', 'phi_3'] = -2*np.pi
# Upper bounds on states
mpc.bounds['upper','_x', 'phi_1'] = 2*np.pi
mpc.bounds['upper','_x', 'phi_2'] = 2*np.pi
mpc.bounds['upper','_x', 'phi_3'] = 2*np.pi

# Lower bounds on inputs:
mpc.bounds['lower','_u', 'phi_m_1_set'] = -2*np.pi
mpc.bounds['lower','_u', 'phi_m_2_set'] = -2*np.pi
# Lower bounds on inputs:
mpc.bounds['upper','_u', 'phi_m_1_set'] = 2*np.pi
mpc.bounds['upper','_u', 'phi_m_2_set'] = 2*np.pi


##Scaling 


mpc.scaling['_x', 'phi_1'] = 2
mpc.scaling['_x', 'phi_2'] = 2
mpc.scaling['_x', 'phi_3'] = 2


##Uncertain parameters


inertia_mass_1 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_2 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_3 = 2.25*1e-4*np.array([1.])

mpc.set_uncertainty_values(
    Theta_1 = inertia_mass_1,
    Theta_2 = inertia_mass_2,
    Theta_3 = inertia_mass_3
)


inertia_mass_1 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_2 = 2.25*1e-4*np.array([1., 0.9, 1.1])
inertia_mass_3 = 2.25*1e-4*np.array([1.])

mpc.set_uncertainty_values(
    Theta_1 = inertia_mass_1,
    Theta_2 = inertia_mass_2,
    Theta_3 = inertia_mass_3
)

##Setup

mpc.setup()

### Configurating the simulator

simulator = do_mpc.simulator.Simulator(model)
# Instead of supplying a dict with the splat operator (**), as with the optimizer.set_param(),
# we can also use keywords (and call the method multiple times, if necessary):

##Simulator parameters

simulator.set_param(t_step = 0.1)

##Uncertain parameters

p_template = simulator.get_p_template()
type(p_template)
p_template.keys()
def p_fun(t_now):
    p_template['Theta_1'] = 2.25e-4
    p_template['Theta_2'] = 2.25e-4
    p_template['Theta_3'] = 2.25e-4
    return p_template
    
simulator.set_p_fun(p_fun)



## Running the optimizer   
u0 = mpc.make_step(x0)


###Creating the model # Add do_mpc to path. This is not necessary if it was installed via pip. # Import do_mpc package: # either 'discrete' or 'continuous' ##Model variables #phi_1 = model.set_variable(var_type='_x', var_name='phi_1', shape=(1,1)) #phi_2 = model.set_variable(var_type='_x', var_name='phi_2', shape=(1,1)) #phi_3 = model.set_variable(var_type='_x', var_name='phi_3', shape=(1,1)) # Variables can also be vectors: # Two states for the desired (set) motor position: # Two additional states for the true motor position: ##Query variable ##Model parameters # As shown in the table above, we can use Long names or short names for the variable type. # 'C' is spring constant and 'd' is damping constant of three disks ##Right-hand-side equation ###Configurating MPC controller ##Optimizer parameters ##Objective function ##Constrains # Lower bounds on states: # Upper bounds on states # Lower bounds on inputs: # Lower bounds on inputs: ##Scaling ##Uncertain parameters ##Setup ### Configurating the simulator # Instead of supplying a dict with the splat operator (**), as with the optimizer.set_param(), # we can also use keywords (and call the method multiple times, if necessary): ##Simulator parameters ##Uncertain parameters ## Running the optimizer