Generated c code for SQP_RTI not working

Hi :wave:

I am using the python interface to generate c code. I am using the c code in my c++ project. I don’t know much about the difference between SQP and SQP RTI. However, from testing in python i found SQP RTI to be faster and more reliable. Therefore I want to use it in my c code.

The solver works fine in python, however the generated code is not working. There are no errors, however the solution violates the model. When I change to SQP it works. I have also tried different solvers. Is there something extra I need to do in the c code when using SQP RTI?

If you are intrested here is a example of when the generated code is not working as expected. I am modeling a car using the bicycle model and I want to follow the line y=0 with velocity = 4.

from acados_template import AcadosOcp, AcadosModel, AcadosOcpSolver
import scipy.linalg
from casadi import SX, vertcat, sin, cos
import numpy as np

def carBicycleModel():
    modelName = "bicycle_model"
    Lf = 2.65
    x1 = SX.sym("x1") # x position
    y1 = SX.sym("y1") # y position
    psi = SX.sym("psi") # angle of car
    v = SX.sym("v") # velocity
    delta = SX.sym("delta") # steering angle
    a = SX.sym("a") # acceleration

    x = vertcat(x1, y1, psi, v)
    u = vertcat(delta, a)

    x1Dot = SX.sym("x1_dot")
    y1Dot = SX.sym("y1_dot")
    psiDot = SX.sym("psi_dot")
    vDot = SX.sym("v_dot")

    xDot = vertcat(x1Dot, y1Dot, psiDot, vDot)

    fExpl = vertcat(
            v*cos(psi),
            v*sin(psi),
            v/Lf*delta,
            a)
    fImpl = xDot - fExpl            

    model = AcadosModel()
    model.name = modelName
    model.f_expl_expr = fExpl
    model.f_impl_expr = fImpl
    model.xdot = xDot
    model.x = x
    model.u = u
    model.p = []
    return model

def main():
    ocp = AcadosOcp()
    ocp.model = carBicycleModel()

    Tf = 2.0
    N = 20
    ocp.dims.N = N

    nx = ocp.model.x.size()[0]
    nu = ocp.model.u.size()[0]
    ny = nx + nu

    ocp.cost.cost_type = "LINEAR_LS"
    ocp.cost.cost_type_e = "LINEAR_LS"
    Q = 2*np.diag([0, 1e1, 1e2, 1e0])
    R = 2*np.diag([1e-1, 1e-1])
    ocp.cost.W = scipy.linalg.block_diag(Q, R)
    ocp.cost.W_e = Q

    Vx = np.zeros((ny, nx))
    Vx[:nx, :nx] = np.eye(nx)
    ocp.cost.Vx = Vx
    ocp.cost.Vx_e = np.eye(nx)

    Vu = np.zeros((ny, nu))
    Vu[nx, 0] = 1
    Vu[nx+1, 1] = 1
    ocp.cost.Vu = Vu

    ocp.cost.yref = np.array([0, 0, 0, 4, 0, 0])
    ocp.cost.yref_e = np.array([0, 0, 0, 4])

    aMax = 1
    deltaMax = 0.57
    ocp.constraints.constr_type = "BGH"
    ocp.constraints.lbu = np.array([-deltaMax, -aMax])
    ocp.constraints.ubu = np.array([deltaMax, aMax])
    ocp.constraints.idxbu = np.array([0, 1])
    ocp.constraints.x0 = np.array([0, 2, -np.pi/4, 0])

    ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" 
    ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
    ocp.solver_options.integrator_type = "ERK"
    ocp.solver_options.nlp_solver_type = "SQP_RTI"
    ocp.solver_options.qp_solver_cond_N = N
    ocp.solver_options.tf = Tf

    ocp_solver = AcadosOcpSolver(ocp, 'acados_ocp_' + ocp.model.name + '.json')
    ocp_solver.solve()
    xTraj = np.ndarray((N, nx))
    for i in range(N):
        xTraj[i, :] = ocp_solver.get(i, "x")
    print("---xTraj---")
    print(xTraj)

main()

Thanks for your help!

Hi,

SQP RTI performs a single SQP iteration on an optimization problem. See this paper https://cdn.syscop.de/publications/Diehl2005c.pdf

It is thus not unexpected that the output of SQP_RTI violates the model equations as you described.

It is rather weird that you seem to not have this behavior in Python. Please check if the solvers are initialized equivalently in your 2 frameworks.

Thanks for your reply.
I looked at the paper you suggested and talked to my professor at NTNU. I realize that the behavior is not unexpected as you said.

I found that the solvers were not initialized equivalently as you suggested. I found that the initial guess for x (x_init) was set to zero in the C-code. However, when I changed x_init to the current state I got the same result in C as I did in python. Perhaps the generated main function should do this as default?