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()