Casadi external() functions in optimization

Feature Requests
,
1

Hi :wave:

I am planning to use a map representation with custom gradients in my MPC problem, that cannot be formulated in the casadi syntax.
I therefore tried to express it in casadi’s C API, that is also used when generating C code from casadi expressions.
This C code can then be compiled to a shared object file and imported back using

f = external("f", "f.so")

Unfortunately using cost expressions obtained in this way does seem to be supported with acados “external” cost functions.
I provided a minimal example below. Note that the C Code generation is only for demonstration and reproduction purposes and cannot be used in my application, since the casadi syntax cannot express my problem.

Code Generation:

from casadi import *
import numpy as np

x = MX.sym("x", 2, 1)   
f = MX.sym("f", 1, 1)
df = MX.sym("df", 1, 2)

generator = CodeGenerator("cost.c")

cost = Function("cost", [x], [x.T @ x])
jac_cost = Function("jac_cost", [x, f], [jacobian(cost(x), x)])
hess_cost = Function("jac_jac_cost", [x, f, df], [hessian(cost(x), x)[0], hessian(cost(x), x)[1]])    

generator.add(cost)
generator.add(jac_cost)
generator.add(hess_cost)

generator.generate()

import subprocess
import time

subprocess.Popen('gcc -fPIC -shared cost.c -o cost.so', shell=True)

time.sleep(0.5)


c = external("cost", "cost.so") 
j = Function("first_derivative",[x], [jacobian(c(x), x)])
h = Function("second_derivative",[x], [hessian(c(x), x)[0]])

point = np.array([1,1])
print(c(point))
print(j(point))
print(h(point))

Acados MPC Script:

import numpy as np
from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
import casadi as cas
import time
import control
import sympy as sp  

from scipy.linalg import expm

import matplotlib.pyplot as plt 


A_c = np.array([[0, 1], [0, 0]])
B_c = np.array([[0], [1]]) 

T_s = 0.033 

A_d = expm(A_c * T_s)
tau =sp.symbols('tau')
B_d = np.array(sp.integrate(sp.exp(sp.Matrix(A_c) *(T_s -  tau)) @ sp.Matrix(B_c), (tau, 0, T_s))).astype(np.float64)

nx = A_d.shape[0]  
nu = B_d.shape[1]  

print("nx:", nx)
print("nu:", nu)

N = 500      
dt = T_s    
Q = np.eye(nx) 
R = np.eye(nu)
K, P, _ = control.dlqr(A_d, B_d, Q, R)
x0 = np.array([10.0, 0.0])  

umin = -10  
umax = 10

ocp = AcadosOcp()


x = cas.MX.sym('x', (nx,1))
u = cas.MX.sym('u', (nu,1))
x_next = cas.mtimes(A_d, x) + cas.mtimes(B_d,  u) 

model = AcadosModel()
model.x = x  
model.u = u 
model.disc_dyn_expr = x_next  
model.name = "linear_mpc"  
ocp.model = model


ocp.dims.N = N
ocp.solver_options.tf = N * dt  


ocp.cost.cost_type = 'EXTERNAL'  
ocp.cost.cost_type_e = 'EXTERNAL'  


y = cas.MX.sym('y', nx) 
x_cost = cas.Function("cost",[y], [y.T @y])   
x_cost_e = cas.external('cost', 'cost.so') 

cost = x_cost(x)+ u.T @ R @ u  
cost_e = x_cost_e(x) # TODO: Change to x_cost(x) to make it work again


ocp.model.cost_expr_ext_cost = cost  
ocp.model.cost_expr_ext_cost_e = cost_e  


ocp.constraints.lbu = np.ones((nu, )) * umin
ocp.constraints.ubu = np.ones((nu, )) * umax
ocp.constraints.idxbu = np.array(range(nu)) 

ocp.constraints.x0 = x0  


ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'EXACT'  
ocp.solver_options.integrator_type = 'DISCRETE' 
ocp.solver_options.nlp_solver_type = 'SQP'

ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')


x = x0
n_sim = 300

ts = np.linspace(0, n_sim * dt, n_sim +1)  

solve_times = []

x_history = [x]
u_history = []

for _ in range(n_sim):

    ocp_solver.set(0, 'lbx', x)
    ocp_solver.set(0, 'ubx', x)
    


    start = time.time()
    status = ocp_solver.solve()
    end = time.time()

    solve_times.append(end - start)
 
    if status != 0:
        print(f"Solver failed with status {status}")


    u_opt = ocp_solver.get(0, 'u')
    u_history.append(u_opt)
  
    x = A_d  @ x + B_d @ u_opt 
    x_history.append(x)

x_history = np.array(x_history)
u_history = np.array(u_history)

solve_times = np.array(solve_times)

print(f"Average solve time: {np.mean(solve_times)}")
print(f"Average Hertz: {1/np.mean(solve_times)}")
print(f"Max solve time: {np.max(solve_times)}")

# Plot results
import matplotlib.pyplot as plt
plt.figure()
plt.plot(ts,x_history)
plt.title("State Trajectory")
plt.xlabel("Seconds")
plt.ylabel("States")
plt.legend(["x1", "x2"])
plt.grid()

plt.figure()
plt.step(ts[:-1], u_history)
plt.title("Control Input")
plt.xlabel("Seconds")
plt.ylabel("u")
plt.legend(["u1"])
plt.grid()

plt.show()

The execution of this results in the following error.

linear_mpc_cost_ext_cost_e_fun_jac.c:(.text+0x160): multiple definition of `casadi_f0_decref'; linear_mpc_cost/linear_mpc_cost_ext_cost_e_fun.o:linear_mpc_cost_ext_cost_e_fun.c:(.text+0xe0): first defined here
collect2: error: ld returned 1 exit status

followed by

OSError: /xxx/c_generated_code/libacados_ocp_solver_linear_mpc.so: cannot open shared object file: No such file or directory

Is it planned to support importing casadi compiled functions in the future?
If not, could you comment on the feasibility of implementing this and maybe point me in the right direction?

Thanks for your help and the amazing work on acados :pray:

Mario

2

Hi :wave:

these custom functions, do you need them to define the cost or dynamics? For the dynamics, something similar – providing custom C functions that match the CasADi C API – is implemented in these examples:

3

Hi,

thanks for the fast response!

I would need the custom functions in the cost and the constraints.
Is this also supported?

Best,
Mario

4

Not at the moment unfortunately.

At least for the cost functions, the required options seem to be there already. If you want to look into extending the functionality, you might want to have a look at this function: