Is it possible to formulate OCP with a neural model for cost function?

Hi Tim! Do you mean that Implementation Acados with L4CasADi could be without cs.types.SimpleNamespace()

Actually, I have formulated my problem without it as I have used to implement Acados before but there are some issues and I couldn’t find out where the problem. So I thought about cs.types.SimpleNamespace()

In the following there is all my implementation for Acados and L4CasADi. Please note that I use L4CasADi only for cost function expression.

from acados_template import AcadosOcp, AcadosOcpSolver , AcadosModel
import casadi as cs
import torch
import l4casadi as l4c
import torch.nn as nn

from casadi import SX, vertcat, sin, cos, tan, exp, if_else, pi , atan , logic_and , sqrt , fabs , atan2 , MX , DM
import numpy as np
import math

class ONF(nn.Module):
    def __init__(self):
        super().__init__()
        self.mlp = nn.Sequential(*[
            nn.Linear(100, 100),
            nn.ReLU(),
            nn.Linear(100, 100),
            nn.ReLU(),
            nn.Linear(100, 100),
            nn.ReLU(),
            nn.Linear(100, 1),
            nn.ReLU()
        ])
        
        self.encoding_layer = nn.Linear(3, 100, bias=False)

    def forward(self, x):
        x = self.encoding_layer(x)
        x = torch.sin_(x)
        x = self.mlp(x)
        return x

def robot_model():
    model_name = "robot_model"

    # yamle file paramters
    # State
    x = MX.sym('x') 
    y = MX.sym('y')   
    v = MX.sym('v')  
    theta = MX.sym('theta') 

    sym_x = vertcat(x, y, v ,theta)

    # Input
    a = MX.sym('a')
    w = MX.sym('w')
    sym_u = vertcat(a, w)

    # Derivative of the States
    x_dot = MX.sym('x_dot')
    y_dot = MX.sym('y_dot')
    theta_dot = MX.sym('theta_dot')
    v_dot = MX.sym('v_dot')
    x_dot = vertcat(x_dot, y_dot, v_dot, theta_dot)

    ## Model of Robot
    f_expl = vertcat(sym_x[2] * cos(sym_x[3]),
                        sym_x[2] * sin(sym_x[3]),
                        sym_u[0],
                        sym_u[1])
    f_impl = x_dot - f_expl

    model = AcadosModel()

    print(model)

    model_loaded = ONF()

    model_loaded.load_state_dict(torch.load('model.pth'))

    print(model_loaded)

    X_test = torch.tensor([[1.4,2.5,0]] , dtype=torch.float32)
    print(model_loaded(X_test))

    l4c_model = l4c.L4CasADi(model_loaded, has_batch=True , name='y_expr' )

    sym = vertcat(x, y,theta)
    print(sym.shape)

    print("model" , l4c_model(sym))

    model.cost_y_expr = vertcat(sym_x, sym_u , l4c_model(vertcat(x,y,theta)))
    model.cost_y_expr_e = vertcat(sym_x, l4c_model(vertcat(x,y,theta)))
    model.f_impl_expr = f_impl
    model.f_expl_expr = f_expl
    model.x = sym_x
    model.xdot = x_dot
    model.u = sym_u
    model.lib = l4c_model
    model.name = "robot"

    return model


def Solver():
    # acados OCP handle
    model = robot_model()
    ocp = AcadosOcp()
    N = 30

    ocp.dims.N = N
    # OCP dimensions
    nx = 4
    nu = 2
    ny = nx + nu
    # OCP costs
    ocp.cost.cost_type = 'NONLINEAR_LS'
    ocp.cost.cost_type_e = 'NONLINEAR_LS'
    ocp.model.cost_y_expr = model.cost_y_expr
    ocp.model.cost_y_expr_e = model.cost_y_expr_e

    w_x = 8 
    w_y = 8
    w_v = 0.00005 
    w_theta = 0.001 
    w_a = 0.01
    w_w = 0.000001 
    w_x_e = 10
    w_y_e = 10
    w_v_e = 10
    w_theta_e = 10 
    # State and input cost
    W_obst = np.array([7])

    W_x = np.array([w_x, w_y, w_v, w_theta, w_a, w_w])
    W = np.diag(np.concatenate([W_x,W_obst]))
    ocp.cost.W = W
    W_xe = np.array([w_x_e, w_y_e, w_v_e, w_theta_e])
    W_e = np.diag(np.concatenate([W_xe,W_obst]))
    ocp.cost.W_e = W_e   
    ocp.cost.yref =  np.zeros([ny+1])
    ocp.cost.yref_e = np.zeros([nx+1])
    
    ocp.constraints.idxbx = np.array([0, 1, 2 , 3 ])
    ocp.constraints.lbx = np.array([-100, -100, 0 , -100])
    ocp.constraints.ubx = np.array([100, 100, 1 , 100])
    ocp.constraints.idxbu = np.array([0, 1])
    ocp.constraints.lbu = np.array([-0.05, -0.3])
    ocp.constraints.ubu = np.array([0.05, 0.3])

                                
    x0 = np.array([0, 0, 0, 0])
    ocp.constraints.x0 = x0

    ocp.model = model

    ocp.solver_options.tf = 12
    ocp.solver_options.qp_solver =  'PARTIAL_CONDENSING_HPIPM' 
    ocp.solver_options.qp_solver_cond_N = 10
    ocp.solver_options.nlp_solver_type = 'SQP'
    ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
    ocp.solver_options.integrator_type = 'ERK'
    ocp.solver_options.levenberg_marquardt = 3.0
    ocp.solver_options.nlp_solver_max_iter = 15
    ocp.solver_options.qp_solver_iter_max = 100
    ocp.solver_options.nlp_solver_tol_stat = 1e2
    ocp.solver_options.nlp_solver_tol_eq = 1e-1
    ocp.solver_options.print_level = 0

    ocp.solver_options.model_external_shared_lib_dir = model.lib.shared_lib_dir
    ocp.solver_options.model_external_shared_lib_name = model.lib.name

    acados_solver = AcadosOcpSolver(ocp)#, json_file="acados_solver.json")

    return acados_solver


Solver()

Sometimes there is this error

File "/home/vladislav/acados/interfaces/acados_template/acados_template/utils.py", line 258, in make_object_json_dumpable
    raise TypeError(f"Cannot make input of type {type(input)} dumpable.")
TypeError: Cannot make input of type <class 'l4casadi.l4casadi.L4CasADi'> dumpable.

and sometimes

File "/home/vladislav/acados/interfaces/acados_template/acados_template/acados_ocp_solver.py", line 666, in ocp_generate_external_functions
    model_dir = os.path.join(code_export_dir, model.name + '_model')
TypeError: unsupported operand type(s) for +: 'NoneType' and 'str'

Hi Muhammad,

the first error which you posted has nothing to do with L4CasADi. You are creating an extra attribute in the AcadosModel instance which is not expected:

model.lib = l4c_model

Acados will try to dump the AcadosModel instance to json and will obviously fail trying to dump the libattribute you created because a L4CasADi object can not be serialized to json. I dont think this is a problem of L4CasADi but unexpected use of the AcadosModel object.

The second error I could not reproduce.

Thank you Tim! It works. Please could you mention what are limitations on Pytorch model that L4CasADi works through, i.e. number of layers, structure of network (MLP , CNN) or any other?

All Best,
Muhammad

You are welcome! Generally it should work on all differentiable models which are traceable by PyTorch’s JIT tracer/compiler. However, I suspect there will be “lots” of small problems with scripting special components. E.g. I had to adapt the scripting process to make LayerNorms work with L4CasADi. If a certain Model / Model part is not working wih L4CasADi please file a issue on Github.

2 Likes