From 3cdb327d8163aae65cd393b660a5a52c552f183c Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <1450062579@qq.com> Date: Wed, 18 Jun 2025 18:46:36 +0200 Subject: [PATCH 1/3] fix partial bounds --- .../casadi_tests/test_casadi_get_set.py | 123 ++++++++++++++++++ .../acados_casadi_ocp_solver.py | 36 +++-- 2 files changed, 148 insertions(+), 11 deletions(-) create mode 100644 examples/acados_python/casadi_tests/test_casadi_get_set.py diff --git a/examples/acados_python/casadi_tests/test_casadi_get_set.py b/examples/acados_python/casadi_tests/test_casadi_get_set.py new file mode 100644 index 0000000000..267d559ba8 --- /dev/null +++ b/examples/acados_python/casadi_tests/test_casadi_get_set.py @@ -0,0 +1,123 @@ +import sys +sys.path.insert(0, '../getting_started') +import numpy as np +import casadi as ca +from typing import Union + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosCasadiOcpSolver +from pendulum_model import export_pendulum_ode_model + + +def get_x_u_traj(ocp_solver: Union[AcadosOcpSolver, AcadosCasadiOcpSolver], N_horizon: int): + ocp = ocp_solver.acados_ocp if isinstance(ocp_solver, AcadosOcpSolver) else ocp_solver.ocp + simX = np.zeros((N_horizon+1, ocp.dims.nx)) + simU = np.zeros((N_horizon, ocp.dims.nu)) + for i in range(N_horizon): + simX[i,:] = ocp_solver.get(i, "x") + simU[i,:] = ocp_solver.get(i, "u") + simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") + + return simX, simU + +def formulate_ocp(Tf: float = 1.0, N: int = 20)-> AcadosOcp: + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + # set h constraints + ocp.model.con_h_expr_0 = ca.norm_2(model.x) + ocp.constraints.lh_0 = np.array([0]) + ocp.constraints.uh_0 = np.array([3.16]) + + nx = model.x.rows() + nu = model.u.rows() + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.u) + ocp.cost.yref = np.zeros((nx+nu,)) + ocp.cost.W = ca.diagcat(Q_mat, R_mat).full() + + # terminal cost + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.yref_e = np.zeros((nx,)) + ocp.model.cost_y_expr_e = model.x + ocp.cost.W_e = Q_mat + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0, np.pi, 0, 0]) # initial state + ocp.constraints.idxbx_0 = np.array([0, 1, 2, 3]) + + # set partial bounds for state + ocp.constraints.lbx = np.array([-10,-10]) + ocp.constraints.ubx = np.array([10,10]) + ocp.constraints.idxbx = np.array([0,3]) + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization + + return ocp + +def main(): + N_horizon = 20 + Tf = 1.0 + ocp = formulate_ocp(Tf, N_horizon) + + initial_iterate = ocp.create_default_initial_iterate() + + ## solve using acados + # create acados solver + ocp_solver = AcadosOcpSolver(ocp,verbose=False) + ocp_solver.load_iterate_from_obj(initial_iterate) + # solve with acados + status = ocp_solver.solve() + # get solution + simX, simU = get_x_u_traj(ocp_solver, N_horizon) + result = ocp_solver.store_iterate_to_obj() + lam = ocp_solver.get_flat("lam") + pi = ocp_solver.get_flat("pi") + + # ## solve using casadi + casadi_ocp_solver = AcadosCasadiOcpSolver(ocp=ocp,solver="ipopt",verbose=False) + casadi_ocp_solver.load_iterate_from_obj(result) + casadi_ocp_solver.solve() + x_casadi_sol, u_casadi_sol = get_x_u_traj(casadi_ocp_solver, N_horizon) + lam_casadi = casadi_ocp_solver.get_flat("lam") + pi_casadi = casadi_ocp_solver.get_flat("pi") + + # evaluate difference + diff_x = np.linalg.norm(x_casadi_sol - simX) + print(f"Difference between casadi and acados solution in x: {diff_x}") + diff_u = np.linalg.norm(u_casadi_sol - simU) + print(f"Difference between casadi and acados solution in u: {diff_u}") + diff_lam = np.linalg.norm(lam_casadi - lam) + print(f"Difference between casadi and acados solution in lam: {diff_lam}") + diff_pi = np.linalg.norm(pi_casadi - pi) + print(f"Difference between casadi and acados solution in pi: {diff_pi}") + + test_tol = 1e-4 + if diff_x > test_tol or diff_u > test_tol or diff_lam > test_tol or diff_pi > test_tol: + raise ValueError(f"Test failed: difference between casadi and acados solution should be smaller than {test_tol}, but got {diff_x} and {diff_u} and {diff_lam} and {diff_pi}.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 18f26fd405..4a158543aa 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -55,6 +55,10 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): # indices of variables within w 'x_in_w': [], 'u_in_w': [], + # indices of state bounds within lam_x(lam_w) in casadi formulation + 'lam_x_in_lam_w':[], + # indices of control bounds within lam_x(lam_w) in casadi formulation + 'lam_u_in_lam_w': [], # indices of dynamic constraints within g in casadi formulation 'pi_in_lam_g': [], # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation @@ -96,18 +100,28 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): lb_xtraj_node[0][constraints.idxbx_0] = constraints.lbx_0 ub_xtraj_node[0][constraints.idxbx_0] = constraints.ubx_0 offset = 0 + index_map['lam_x_in_lam_w'].append(list(offset + constraints.idxbx_0)) + offset += dims.nbx_0 +dims.nbu for i in range(1, N_horizon): lb_xtraj_node[i][constraints.idxbx] = constraints.lbx ub_xtraj_node[i][constraints.idxbx] = constraints.ubx + index_map['lam_x_in_lam_w'].append(list(offset + constraints.idxbx)) + offset += dims.nx + dims.nbu lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e + index_map['lam_x_in_lam_w'].append(list(offset + constraints.idxbx_e)) + offset += dims.nx # setup control bounds lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] + offset = 0 for i in range(N_horizon): lb_utraj_node[i][constraints.idxbu] = constraints.lbu ub_utraj_node[i][constraints.idxbu] = constraints.ubu + offset += dims.nx + index_map['lam_u_in_lam_w'].append(list(offset + constraints.idxbu)) + offset += dims.nu ### Concatenate primal variables and bounds # w = [x0, u0, x1, u1, ...] @@ -492,15 +506,15 @@ def get(self, stage: int, field: str): return -self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() elif field == 'lam': if stage == 0: - bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx_0 else np.empty((0, 1)) - bu_lam = self.nlp_sol_lam_x[self.index_map['u_in_w'][stage]] if dims.nbu else np.empty((0, 1)) + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_x_in_lam_w'][stage]] + bu_lam = self.nlp_sol_lam_x[self.index_map['lam_u_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage < dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx else np.empty((0, 1)) - bu_lam = self.nlp_sol_lam_x[self.index_map['u_in_w'][stage]] if dims.nbu else np.empty((0, 1)) + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_x_in_lam_w'][stage]] + bu_lam = self.nlp_sol_lam_x[self.index_map['lam_u_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage == dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['x_in_w'][stage]] if dims.nbx_e else np.empty((0, 1)) + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_x_in_lam_w'][stage]] bu_lam = np.empty((0, 1)) g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] @@ -673,17 +687,17 @@ def set(self, stage: int, field: str, value_: np.ndarray): n_ghphi = dims.ng_e + dims.nh_e + dims.nphi_e offset_u = (nbx+nbu+n_ghphi) - lbu_lam = value_[:nbu] if nbu else np.empty((dims.nu,)) - lbx_lam = value_[nbu:nbu+nbx] if nbx else np.empty((dims.nx,)) + lbu_lam = value_[:nbu] + lbx_lam = value_[nbu:nbu+nbx] lg_lam = value_[nbu+nbx:nbu+nbx+n_ghphi] - ubu_lam = value_[offset_u:offset_u+nbu] if nbu else np.empty((dims.nu,)) - ubx_lam = value_[offset_u+nbu:offset_u+nbu+nbx] if nbx else np.empty((dims.nx,)) + ubu_lam = value_[offset_u:offset_u+nbu] + ubx_lam = value_[offset_u+nbu:offset_u+nbu+nbx] ug_lam = value_[offset_u+nbu+nbx:offset_u+nbu+nbx+n_ghphi] if stage != dims.N: - self.lam_x0[self.index_map['x_in_w'][stage]+self.index_map['u_in_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) + self.lam_x0[self.index_map['lam_x_in_lam_w'][stage]+self.index_map['lam_u_in_lam_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam else: - self.lam_x0[self.index_map['x_in_w'][stage]] = ubx_lam-lbx_lam + self.lam_x0[self.index_map['lam_x_in_lam_w'][stage]] = ubx_lam-lbx_lam self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam elif field in ['sl', 'su']: # do nothing for now, only empty is supported From 908f36e3d4a9dafc3fb3d4aa2212db27d9b642a2 Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <1450062579@qq.com> Date: Thu, 19 Jun 2025 13:44:22 +0200 Subject: [PATCH 2/3] merge loop and fix --- .../acados_casadi_ocp_solver.py | 66 ++++++++++--------- 1 file changed, 34 insertions(+), 32 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 4a158543aa..267e075ce5 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -56,9 +56,9 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): 'x_in_w': [], 'u_in_w': [], # indices of state bounds within lam_x(lam_w) in casadi formulation - 'lam_x_in_lam_w':[], + 'lam_bx_in_lam_w':[], # indices of control bounds within lam_x(lam_w) in casadi formulation - 'lam_u_in_lam_w': [], + 'lam_bu_in_lam_w': [], # indices of dynamic constraints within g in casadi formulation 'pi_in_lam_g': [], # indicies to [g, h, phi] in acados formulation within lam_g in casadi formulation @@ -94,34 +94,36 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): # parameters ptraj_node = [ca_symbol(f'p{i}', dims.np, 1) for i in range(N_horizon)] - # setup state bounds + # setup state and control bounds lb_xtraj_node = [-np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] ub_xtraj_node = [np.inf * ca.DM.ones((dims.nx, 1)) for _ in range(N_horizon+1)] - lb_xtraj_node[0][constraints.idxbx_0] = constraints.lbx_0 - ub_xtraj_node[0][constraints.idxbx_0] = constraints.ubx_0 - offset = 0 - index_map['lam_x_in_lam_w'].append(list(offset + constraints.idxbx_0)) - offset += dims.nbx_0 +dims.nbu - for i in range(1, N_horizon): - lb_xtraj_node[i][constraints.idxbx] = constraints.lbx - ub_xtraj_node[i][constraints.idxbx] = constraints.ubx - index_map['lam_x_in_lam_w'].append(list(offset + constraints.idxbx)) - offset += dims.nx + dims.nbu - lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e - ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e - index_map['lam_x_in_lam_w'].append(list(offset + constraints.idxbx_e)) - offset += dims.nx - - # setup control bounds lb_utraj_node = [-np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] ub_utraj_node = [np.inf * ca.DM.ones((dims.nu, 1)) for _ in range(N_horizon)] offset = 0 - for i in range(N_horizon): - lb_utraj_node[i][constraints.idxbu] = constraints.lbu - ub_utraj_node[i][constraints.idxbu] = constraints.ubu - offset += dims.nx - index_map['lam_u_in_lam_w'].append(list(offset + constraints.idxbu)) - offset += dims.nu + for i in range(0, N_horizon+1): + if i == 0: + lb_xtraj_node[0][constraints.idxbx_0] = constraints.lbx_0 + ub_xtraj_node[0][constraints.idxbx_0] = constraints.ubx_0 + index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx_0)) + offset += dims.nx + lb_utraj_node[i][constraints.idxbu] = constraints.lbu + ub_utraj_node[i][constraints.idxbu] = constraints.ubu + index_map['lam_bu_in_lam_w'].append(list(offset + constraints.idxbu)) + offset += dims.nu + elif i < N_horizon: + lb_xtraj_node[i][constraints.idxbx] = constraints.lbx + ub_xtraj_node[i][constraints.idxbx] = constraints.ubx + index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx)) + offset += dims.nx + lb_utraj_node[i][constraints.idxbu] = constraints.lbu + ub_utraj_node[i][constraints.idxbu] = constraints.ubu + index_map['lam_bu_in_lam_w'].append(list(offset + constraints.idxbu)) + offset += dims.nu + elif i == N_horizon: + lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e + ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e + index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx_e)) + offset += dims.nx ### Concatenate primal variables and bounds # w = [x0, u0, x1, u1, ...] @@ -506,15 +508,15 @@ def get(self, stage: int, field: str): return -self.nlp_sol_lam_g[self.index_map['pi_in_lam_g'][stage]].flatten() elif field == 'lam': if stage == 0: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_x_in_lam_w'][stage]] - bu_lam = self.nlp_sol_lam_x[self.index_map['lam_u_in_lam_w'][stage]] + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] + bu_lam = self.nlp_sol_lam_x[self.index_map['lam_bu_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage < dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_x_in_lam_w'][stage]] - bu_lam = self.nlp_sol_lam_x[self.index_map['lam_u_in_lam_w'][stage]] + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] + bu_lam = self.nlp_sol_lam_x[self.index_map['lam_bu_in_lam_w'][stage]] g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] elif stage == dims.N: - bx_lam = self.nlp_sol_lam_x[self.index_map['lam_x_in_lam_w'][stage]] + bx_lam = self.nlp_sol_lam_x[self.index_map['lam_bx_in_lam_w'][stage]] bu_lam = np.empty((0, 1)) g_lam = self.nlp_sol_lam_g[self.index_map['lam_gnl_in_lam_g'][stage]] @@ -694,10 +696,10 @@ def set(self, stage: int, field: str, value_: np.ndarray): ubx_lam = value_[offset_u+nbu:offset_u+nbu+nbx] ug_lam = value_[offset_u+nbu+nbx:offset_u+nbu+nbx+n_ghphi] if stage != dims.N: - self.lam_x0[self.index_map['lam_x_in_lam_w'][stage]+self.index_map['lam_u_in_lam_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) + self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]+self.index_map['lam_bu_in_lam_w'][stage]] = np.concatenate((ubx_lam-lbx_lam, ubu_lam-lbu_lam)) self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam else: - self.lam_x0[self.index_map['lam_x_in_lam_w'][stage]] = ubx_lam-lbx_lam + self.lam_x0[self.index_map['lam_bx_in_lam_w'][stage]] = ubx_lam-lbx_lam self.lam_g0[self.index_map['lam_gnl_in_lam_g'][stage]] = ug_lam-lg_lam elif field in ['sl', 'su']: # do nothing for now, only empty is supported From 5dffa50594707f63b94486ee2dbcb8b1d61ba40c Mon Sep 17 00:00:00 2001 From: Jingtao Xiong <1450062579@qq.com> Date: Thu, 19 Jun 2025 17:09:23 +0200 Subject: [PATCH 3/3] refine loop & modify Cmakelist and test --- .../casadi_tests/test_casadi_get_set.py | 4 +++- interfaces/CMakeLists.txt | 4 ++++ .../acados_template/acados_casadi_ocp_solver.py | 17 +++++++---------- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/examples/acados_python/casadi_tests/test_casadi_get_set.py b/examples/acados_python/casadi_tests/test_casadi_get_set.py index 267d559ba8..c34e9fc5dd 100644 --- a/examples/acados_python/casadi_tests/test_casadi_get_set.py +++ b/examples/acados_python/casadi_tests/test_casadi_get_set.py @@ -25,6 +25,8 @@ def formulate_ocp(Tf: float = 1.0, N: int = 20)-> AcadosOcp: # set model model = export_pendulum_ode_model() + # add dummy control + model.u = ca.vertcat(model.u, ca.SX.sym('dummy_u')) ocp.model = model # set h constraints @@ -41,7 +43,7 @@ def formulate_ocp(Tf: float = 1.0, N: int = 20)-> AcadosOcp: # cost matrices Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) - R_mat = 2*np.diag([1e-2]) + R_mat = 2*np.diag([1e-2, 1e-2]) # path cost ocp.cost.cost_type = 'NONLINEAR_LS' diff --git a/interfaces/CMakeLists.txt b/interfaces/CMakeLists.txt index 60421f9449..006830e90c 100644 --- a/interfaces/CMakeLists.txt +++ b/interfaces/CMakeLists.txt @@ -352,6 +352,10 @@ add_test(NAME python_pendulum_ocp_example_cmake add_test(NAME python_convex_ocp_with_onesided_constraints COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/convex_ocp_with_onesided_constraints python main_convex_onesided.py) + + add_test(NAME python_casadi_get_set_example + COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/casadi_tests + python test_casadi_get_set.py) # Sim add_test(NAME python_pendulum_ext_sim_example diff --git a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py index 267e075ce5..4366739692 100644 --- a/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_casadi_ocp_solver.py @@ -102,28 +102,25 @@ def __init__(self, ocp: AcadosOcp, with_hessian=False): offset = 0 for i in range(0, N_horizon+1): if i == 0: - lb_xtraj_node[0][constraints.idxbx_0] = constraints.lbx_0 - ub_xtraj_node[0][constraints.idxbx_0] = constraints.ubx_0 + lb_xtraj_node[i][constraints.idxbx_0] = constraints.lbx_0 + ub_xtraj_node[i][constraints.idxbx_0] = constraints.ubx_0 index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx_0)) offset += dims.nx - lb_utraj_node[i][constraints.idxbu] = constraints.lbu - ub_utraj_node[i][constraints.idxbu] = constraints.ubu - index_map['lam_bu_in_lam_w'].append(list(offset + constraints.idxbu)) - offset += dims.nu elif i < N_horizon: lb_xtraj_node[i][constraints.idxbx] = constraints.lbx ub_xtraj_node[i][constraints.idxbx] = constraints.ubx index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx)) offset += dims.nx - lb_utraj_node[i][constraints.idxbu] = constraints.lbu - ub_utraj_node[i][constraints.idxbu] = constraints.ubu - index_map['lam_bu_in_lam_w'].append(list(offset + constraints.idxbu)) - offset += dims.nu elif i == N_horizon: lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e index_map['lam_bx_in_lam_w'].append(list(offset + constraints.idxbx_e)) offset += dims.nx + if i < N_horizon: + lb_utraj_node[i][constraints.idxbu] = constraints.lbu + ub_utraj_node[i][constraints.idxbu] = constraints.ubu + index_map['lam_bu_in_lam_w'].append(list(offset + constraints.idxbu)) + offset += dims.nu ### Concatenate primal variables and bounds # w = [x0, u0, x1, u1, ...]