acados · FreyJo · Jun 19, 2025 · Jun 18, 2025 · Jun 19, 2025 · Jun 19, 2025
diff --git a/examples/acados_python/casadi_tests/test_casadi_get_set.py b/examples/acados_python/casadi_tests/test_casadi_get_set.py
@@ -0,0 +1,125 @@
+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()
+    # add dummy control
+    model.u = ca.vertcat(model.u, ca.SX.sym('dummy_u'))
+    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, 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()
diff --git 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
@@ -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_bx_in_lam_w':[],
+            # indices of control bounds within lam_x(lam_w) in casadi formulation
+            '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
@@ -90,24 +94,33 @@ 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
-        for i in range(1, N_horizon):
-            lb_xtraj_node[i][constraints.idxbx] = constraints.lbx
-            ub_xtraj_node[i][constraints.idxbx] = constraints.ubx
-        lb_xtraj_node[-1][constraints.idxbx_e] = constraints.lbx_e
-        ub_xtraj_node[-1][constraints.idxbx_e] = constraints.ubx_e
-
-        # 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)]
-        for i in range(N_horizon):
-            lb_utraj_node[i][constraints.idxbu] = constraints.lbu
-            ub_utraj_node[i][constraints.idxbu] = constraints.ubu
+        offset = 0
+        for i in range(0, N_horizon+1):
+            if i == 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
+            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
+            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, ...]
@@ -492,15 +505,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_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['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_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['x_in_w'][stage]] if dims.nbx_e else np.empty((0, 1))
+                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]]
 
@@ -673,17 +686,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_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['x_in_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