External objective function with algebraic variables

Hi Jonathan, :wave:

I am using acados to solve an ocp in power grids, and for now, I am using the well know benchmark 9bus system. This model is a DAE system with 12 dynamic states, 27 algebraic states, and 6 inputs. I am also using the Matlab interface with Casadi 3.5.5. I want to use an objective function that includes some algebraic variables, it works when I use the built-in objective function (‘linear_ls’), but it crashes when I write the same function as external objective function (‘ext_cost’). If I use only the dynamic states and inputs the external objective function works well. When I use the algebraic states in the external objective I got the following error message:

Error using casadi.Function/generate (line 1245)
.../casadi/core/sx_function.cpp:149: Code generation of 'dae_9Bus_cost_ext_cost_fun' is not
possible since variables [ex_0, ex_1, ex_2, ey_0, ey_1, ey_2, ix_0, ix_1, ix_2, iy_0, iy_1,
iy_2, id_0, id_1, id_2, iq_0, iq_1, iq_2, ed_0, ed_1, ed_2, eq_0, eq_1, eq_2, Pe_0, Pe_1,
Pe_2] are free.
Error in casadi.Function/subsref (line 1753)
        [varargout{1:nargout}] = builtin('subsref',self,s);
Error in generate_c_code_ext_cost (line 90)
    ext_cost_fun.generate([model_name,'_cost_ext_cost_fun'], casadi_opts);
Error in ocp_generate_casadi_ext_fun (line 150)
    generate_c_code_ext_cost(model_struct, opts_struct);
Error in acados_ocp (line 194)
                ocp_generate_casadi_ext_fun(obj.model_struct, obj.opts_struct);
Error in ocp_9bus (line 153)
ocp = acados_ocp(ocp_model, ocp_opts);

You can reproduce this error with the following simulation files. The different options for the objective function can be selected in lines 35 and 36 of the second file.

function model = dae_9bus()   
    %% CasADi
    import casadi.*
    model_name_prefix = 'dae_9bus';
    
    %% Parameters 
    ws=2*pi*60;
    H = [ 23.64 6.40 3.01 ]';
    D = [ 0 0 0 ]';
    Xd = [ 0.1460 0.8958 1.3125 ]';
    Xq = [ 0.0969 0.8465 1.2578 ]';
    Xds = [ 0.0608 0.1198 0.1813 ]';
    % Xqs = [ 0.0969 0.1969 0.2500 ]'; %for updating Y OR
    Xqs = Xds; % so that Y is a constant matrix, see paper page 3 
    Tds0 = [ 8.9600 6.0000 5.8900 ]';
    Tqs0 = [ 0.6000 0.5350 0.6000 ]';
    Ra = [ 0 0 0 ]';     
    Y=[ 0.8455  2.9883  0.2871 -1.5129  0.2096 -1.2259
        -2.9883  0.8455  1.5129  0.2871  1.2256  0.2096
         0.2871 -1.5129  0.4200  2.7239  0.2133 -1.0879
         1.5129  0.2871 -2.7239  0.4200  1.0879  0.2133
         0.2096 -1.2256  0.2133 -1.0879  0.2770  2.3681
         1.2256  0.2096  1.0879  0.2133 -2.3681  0.2770
        ];
    
    %% initial values
    x0 = [ 9.8598e-02
           1.0343e+00
           9.1743e-01
                    0
                    0
                    0
           8.9957e-01
           6.7692e-01
           5.9303e-01
           2.5647e-02
           5.6773e-01
           5.4621e-01];
       
   z0 = [  8.9773e-01
           8.3395e-01
           7.9420e-01
           6.3030e-02
           2.9167e-01
           1.3887e-01
           7.4177e-01
           1.3254e+00
           5.2034e-01
          -2.8150e-01
           1.2081e-01
           2.4066e-01
           3.5315e-01
           1.0775e+00
           2.6689e-01
           7.1045e-01
           7.8125e-01
           5.0739e-01
          -1.7548e-02
           4.7414e-01
           4.5422e-01
           8.7810e-01
           5.4783e-01
           5.4465e-01
           6.1765e-01
           9.3888e-01
           3.9758e-01];

    u0 = [ 6.1765e-01
           9.3888e-01
           3.9758e-01
           9.2966e-01
           1.5131e+00
           8.9493e-01];
       
    %% Set up States & Controls
    % Differential States
    delta = SX.sym('delta',3,1);  
    w     = SX.sym('w',3,1);
    eqs   = SX.sym('eqs',3,1);     
    eds   = SX.sym('eds',3,1);
    x = vertcat(delta, w, eds, eqs);
    nx = length(x);
    
    % Algebraic states
    ex = SX.sym('ex',3,1);     
    ey = SX.sym('ey',3,1);
    ix = SX.sym('ix',3,1);     
    iy = SX.sym('iy',3,1);
    id = SX.sym('id',3,1);     
    iq = SX.sym('iq',3,1);
    eq = SX.sym('eq',3,1);     
    ed = SX.sym('ed',3,1);
    Pe = SX.sym('Pe',3,1);
    z = vertcat(ex, ey, ix, iy, id, iq, ed, eq, Pe);
    nz = length(z);
    
    % Controls % applied torque    
    Pm = SX.sym('Pm',3,1);  
    Efq = SX.sym('Efq',3,1); 
    u = vertcat(Pm, Efq);
    nu = length(u);
    
    %% xdot
    delta_dot = SX.sym('delta_dot',3);     % Differential States
    w_dot     = SX.sym('w_dot',3);
    eqs_dot   = SX.sym('eqs_dot',3);     
    eds_dot   = SX.sym('eds_dot',3);   
    xdot = [delta_dot; w_dot; eqs_dot; eds_dot];
        
	%% cost    
    W_x = 1*diag(ones(1,nx));
    W_u = 1e-1*diag(ones(1,nu));
    W_z = 1*diag(ones(1,nz));
    W_e = 1*W_x;
    expr_ext_cost_e = (x-x0)'* W_e *(x-x0);
    expr_ext_cost_xu = (x-x0)'* W_x *(x-x0) + (u-u0)'* W_u *(u-u0);
    expr_ext_cost_xuz = (x-x0)'* W_x *(x-x0) + (z-z0)'*W_z*(z-z0) + (u-u0)'* W_u *(u-u0);
    

    %% Dynamics: implicit DAE formulation (index-1)
    f_impl = vertcat(delta_dot - ws.*w, ...  
                     w_dot - (Pm-Pe-D.*w)./(2*H), ...  
                     eqs_dot - (Efq-(Xd-Xds).*id-eqs)./Tds0, ...  
                     eds_dot - ((Xq-Xqs).*iq-eds)./Tqs0, ... 
                     ex - (sin(delta).*eds + cos(delta).*eqs), ...       
                     ey - (-cos(delta).*eds + sin(delta).*eqs), ...       
                     [ix; iy] - Y*[ex;ey], ... 
                     id - (sin(delta).*ix - cos(delta).*iy), ...
                     iq - (cos(delta).*ix + sin(delta).*iy), ...
                     ed - (eds - Ra.*id + Xqs.*iq), ...
                     eq - (eqs - Xds.*id - Ra.*iq), ...
                     Pe - (ed.*id + eq.*iq));

    %% model struct

    model.expr_f_impl = f_impl;
    model.sym_x = x;
    model.sym_xdot = xdot;
    model.sym_u = u;
    model.sym_z = z;
    model.name = model_name_prefix;
    model.expr_ext_cost_xu = expr_ext_cost_xu;
    model.expr_ext_cost_xuz = expr_ext_cost_xuz;
    model.expr_ext_cost_e = expr_ext_cost_e;
    model.nx = nx;
    model.nz = nz;
    model.nu = nu;
    model.x0 = x0;
    model.z0 = z0;
    model.u0 = u0;
    
end

clear all
close all
clc

model_name = 'dae_9Bus';

%% options
compile_interface = 'auto'; % true, false
codgen_model = 'true'; % true, false

% ocp
N = 20;
nlp_solver = 'sqp'; % sqp, sqp_rti
nlp_solver_exact_hessian = 'true';
regularize_method = 'no_regularize';
nlp_solver_max_iter = 10;
nlp_solver_tol_stat = 1e-12;
nlp_solver_tol_eq   = 1e-12;
nlp_solver_tol_ineq = 1e-12;
nlp_solver_tol_comp = 1e-12;
nlp_solver_ext_qp_res = 1;
nlp_solver_step_length = 0.7;
qp_solver = 'partial_condensing_hpipm'; % partial_condensing_hpipm
qp_solver_cond_N = 5;
qp_solver_warm_start = 2;
qp_solver_cond_ric_alg = 1; % 0: dont factorize hessian in the condensing; 1: factorize
qp_solver_ric_alg = 1; % HPIPM specific
ocp_sim_method = 'irk'; % irk, irk_gnsf
ocp_sim_method_num_stages = 2; % scalar or vector of size ocp_N;
ocp_sim_method_num_steps = 2; % scalar or vector of size ocp_N;
ocp_sim_method_newton_iter = 2; % scalar or vector of size ocp_N;

% selectors for example variants
cost_type = 'external'; % 'internal' or 'external'
ext_variables = 1; % 0: xu or 1: xuz

% get model
model = dae_9bus;
x0 = model.x0;
z0 = model.z0;
u0 = model.u0; 
nx = model.nx;
nz = model.nz;
nu = model.nu; 
ny = nx+nu+nz;
h = 0.01;
T = N*h;

%% acados ocp model
ocp_model = acados_ocp_model();
ocp_model.set('T', T);
ocp_model.set('name', model_name);

% symbolics
ocp_model.set('sym_x', model.sym_x);
ocp_model.set('sym_u', model.sym_u);
ocp_model.set('sym_xdot', model.sym_xdot);
ocp_model.set('sym_z', model.sym_z);

% cost
if cost_type == 'external'
    ocp_model.set('cost_type', 'ext_cost');
    ocp_model.set('cost_type_e', 'ext_cost');
    if ext_variables == 0
        ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost_xu);
    elseif ext_variables == 1
        ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost_xuz);
    end
    
    ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e);
else
    % state-to-output matrix in lagrange term
    Vx = eye(ny, nx); 
    % input-to-output matrix in lagrange term
    Vu = zeros(ny, nu);
    Vu(nx+1:nx+nu, :) = eye(nu);
    % algebraic-to-output matrix in lagrange term
    Vz = zeros(ny, nz);
    Vz(nx+nu+1:end, :) = eye(nz); 
    % state-to-output matrix in mayer term
    Vx_e = eye(nx); 
    % weight matrix in lagrange term
    Wx = 1*ones(1,nx);
    Wu = 1*ones(1,nu);
    Wz = 1*ones(1,nz);
    W = diag([Wx,Wu,Wz]);
    yr = [x0;u0;z0];
    % weight matrix in mayer term
    W_e = 1*diag(Wx); 
    yr_e = x0; % output reference in mayer term
    %ocp_model struct
    ocp_model.set('cost_type', 'linear_ls');
    ocp_model.set('cost_type_e', 'linear_ls');
    ocp_model.set('cost_Vu', Vu);
    ocp_model.set('cost_Vz', Vz);
    ocp_model.set('cost_Vx', Vx);
    ocp_model.set('cost_Vx_e', Vx_e);
    ocp_model.set('cost_W_e', W_e);
    ocp_model.set('cost_W', W);
    ocp_model.set('cost_y_ref', yr);
    ocp_model.set('cost_y_ref_e', yr_e);
end

% dynamics
ocp_model.set('dyn_type', 'implicit');
ocp_model.set('dyn_expr_f', model.expr_f_impl);

% constraints
ocp_model.set('constr_x0', x0);
x_traj_init = repmat(x0, 1, N + 1);
u_traj_init = u0.*ones(model.nu, N);
z_traj_init = z0.*ones(model.nz, N);
xdot_traj_init = 0*ones(model.nx, N);

%% acados ocp opts
ocp_opts = acados_ocp_opts();

ocp_opts.set('compile_interface', compile_interface);
ocp_opts.set('codgen_model', codgen_model);
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian);
ocp_opts.set('regularize_method', regularize_method);
ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res);
ocp_opts.set('nlp_solver_step_length', nlp_solver_step_length);
if (strcmp(nlp_solver, 'sqp'))
    ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter);
    ocp_opts.set('nlp_solver_tol_stat', nlp_solver_tol_stat);
    ocp_opts.set('nlp_solver_tol_eq', nlp_solver_tol_eq);
    ocp_opts.set('nlp_solver_tol_ineq', nlp_solver_tol_ineq);
    ocp_opts.set('nlp_solver_tol_comp', nlp_solver_tol_comp);
end
ocp_opts.set('qp_solver', qp_solver);
if (strcmp(qp_solver, 'partial_condensing_hpipm'))
    ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N);
    ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg);
    ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg);
    ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start);
end
ocp_opts.set('sim_method', ocp_sim_method);
ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages);
ocp_opts.set('sim_method_num_steps', ocp_sim_method_num_steps);
ocp_opts.set('sim_method_newton_iter', ocp_sim_method_newton_iter);

ocp_opts.set('exact_hess_dyn', 1);
ocp_opts.set('exact_hess_cost', 1);
ocp_opts.set('exact_hess_constr', 1);



%% acados ocp
ocp = acados_ocp(ocp_model, ocp_opts);

ocp.set('init_x', x_traj_init);
ocp.set('init_u', u_traj_init);
ocp.set('init_z', z_traj_init);
ocp.set('init_xdot', xdot_traj_init);

ocp.solve();

stat = ocp.get('stat');

ocp.print('stat')

status = ocp.get('status');
sqp_iter = ocp.get('sqp_iter');
sqp_time = ocp.get('time_tot');

format short e

% get solution for initialization of next NLP
x_traj = ocp.get('x');
u_traj = ocp.get('u');
z_traj = ocp.get('z');

%plot result
figure;
Nsub = 3;
subplot(Nsub, 1, 1);
plot(x_traj');
legend('x');

subplot(Nsub, 1, 2);
plot(z_traj');
legend('z');

subplot(Nsub, 1, 3);
plot(u_traj');
legend('u');

Can you help me to solve this issue?

Best regards, :pray:
Paulo

Hi Paulo,

unfortunately, the external cost function module does not support algebraic variables (yet).
See: acados/ocp_nlp_cost_external.c at 5586235186e4ff08d396821a752392ebcc82efde · acados/acados · GitHub

I agree that would be very useful.
There should have been a proper error catching for that.
I hope to find time to implement that properly very soon, will post an update here then!

Best,
Jonathan

Hi Jonathan,

Thanks for your fast response. I will wait for this update! It will help a lot to implement real time algorithms to systems with algebraic variables in the objective function.

Best,
Paulo