import matplotlib.pyplot as plt
from numpy import array, diag
from scipy.linalg import block_diag
from casadi import SX, Function, vertcat
from acados import ocp_nlp_function, ocp_nlp_ls_cost, ocp_nlp, ocp_nlp_solver
from models import chen_model
N = 10
ode_fun, nx, nu = chen_model()
nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu, 'ng': N*[1] + [0]})
# ODE Model
step = 0.1
nlp.set_model(ode_fun, step)
# Cost function
Q = diag([1.0, 1.0])
R = 1e-2
x = SX.sym('x', nx)
u = SX.sym('u', nu)
u_N = SX.sym('u', 0)
f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)]))
f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x]))
ls_cost = ocp_nlp_ls_cost(N, N*[f]+[f_N])
ls_cost.ls_cost_matrix = N*[block_diag(Q, R)] + [Q]
nlp.set_cost(ls_cost)
# Constraints
g = ocp_nlp_function(Function('path_constraint', [x, u], [u]))
g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])]))
import matplotlib.pyplot as plt
from numpy import array, diag
from scipy.linalg import block_diag
from casadi import SX, Function, vertcat
from acados import ocp_nlp_function, ocp_nlp_ls_cost, ocp_nlp, ocp_nlp_solver
from models import chen_model
N = 10
ode_fun, nx, nu = chen_model()
nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu, 'ng': N * [1] + [0]})
# ODE Model
step = 0.1
nlp.set_model(ode_fun, step)
# Cost function
Q = diag([1.0, 1.0])
R = 1e-2
x = SX.sym('x', nx)
u = SX.sym('u', nu)
u_N = SX.sym('u', 0)
f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)]))
f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x]))
ls_cost = ocp_nlp_ls_cost(N, N * [f] + [f_N])
ls_cost.ls_cost_matrix = N * [block_diag(Q, R)] + [Q]
nlp.set_cost(ls_cost)
# Constraints
g = ocp_nlp_function(Function('path_constraint', [x, u], [u]))
g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])]))
import matplotlib.pyplot as plt
from numpy import array, diag
from scipy.linalg import block_diag
from casadi import SX, Function, vertcat
from acados import ocp_nlp_function, ocp_nlp_ls_cost, ocp_nlp, ocp_nlp_solver
from models import chen_model
N = 10
ode_fun, nx, nu = chen_model()
nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu})
# ODE Model
step = 0.1
nlp.set_model(ode_fun, step)
# Cost function
Q = diag([1.0, 1.0])
R = 1e-2
x = SX.sym('x', nx)
u = SX.sym('u', nu)
uN = SX.sym('u', 0)
F = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)]))
FN = ocp_nlp_function(Function('ls_costN', [x, uN], [x]))
ls_cost = ocp_nlp_ls_cost(N, N * [F] + [FN])
ls_cost.ls_cost_matrix = N * [block_diag(Q, R)] + [Q]
nlp.set_cost(ls_cost)
# Constraints
g = SX([])
G = ocp_nlp_function(Function('path_constraint', [x, u], [g]))
from numpy import array, diag, eye, zeros
from scipy.linalg import block_diag
from acados import ocp_nlp
from casadi import SX, Function, vertcat
nx, nu = 2, 1
x = SX.sym('x', nx)
u = SX.sym('u', nu)
ode_fun = Function('ode_fun', [x, u], [vertcat(x[1], u)], ['x', 'u'], ['rhs'])
N = 15
nlp = ocp_nlp(N, nx, nu)
nlp.set_dynamics(ode_fun, {'integrator': 'rk4', 'step': 0.1})
q, r = 1, 1
P = eye(nx)
nlp.set_stage_cost(eye(nx + nu), zeros(nx + nu), diag([q, q, r]))
nlp.set_terminal_cost(eye(nx), zeros(nx), P)
x0 = array([1, 1])
nlp.set_field("lbx", 0, x0)
nlp.set_field("ubx", 0, x0)
nlp.initialize_solver("sqp", {"qp_solver": "qpoases"})
output = nlp.solve()
from numpy import array, diag, eye, zeros
from scipy.linalg import block_diag
from acados import ocp_nlp
from casadi import SX, Function, vertcat
nx, nu = 2, 1
x = SX.sym('x', nx)
u = SX.sym('u', nu)
ode_fun = Function('ode_fun', [x, u], [vertcat(x[1], u)], ['x', 'u'], ['rhs'])
N = 15
nlp = ocp_nlp(N, nx, nu)
nlp.set_dynamics(ode_fun, {'integrator': 'rk4', 'step': 0.1})
q, r = 1, 1
P = eye(nx)
nlp.set_stage_cost(eye(nx+nu), zeros(nx+nu), diag([q, q, r]))
nlp.set_terminal_cost(eye(nx), zeros(nx), P)
x0 = array([1, 1])
nlp.set_field("lbx", 0, x0)
nlp.set_field("ubx", 0, x0)
nlp.initialize_solver("sqp", {"qp_solver": "qpoases"})
output = nlp.solve()
