Hello Kaethe,
This is the OCP code which worked for me in terms of assigning the values of the parameters.
clear all
model_path=fullfile(pwd,'..','test_model_with_param')
addpath(model_path)
check_acados_requirements()
% OCP parameters and designated solvers
%Note: OCP is an open-loop control method
%Note: Repeat OCP at each time instant = MPC (closed-loop control)
N=100; %%%%%%%%%%%%%%%5
T=2; %%%%%%%%%%%%%
Ts=T/N; %every time period a control signal is outputed for OCP and currently for MPC too (uniform time grid)
%We will consider currently that MPC outputs control input at this rate too
x0=[0;pi;0;0];%Initial state is that pendulum is stationary and in equilibrium position
nlp_solver='sqp';
qp_solver='partial_condensing_hpipm';
qp_solver_cond_N = 5; % for partial condensing
sim_method = 'erk'; % integrator is explicit runge kutta for OCP
%model
model=test_model_with_param;
nx=model.nx;
nu=model.nu;
ny = size(model.cost_expr_y, 1); % used in simulink example
ny_e = size(model.cost_expr_y_e, 1);
%Create new acados solver for cart-pole model
ocp_model=acados_ocp_model();
%Set characteristics of ocp (problem formulation)
ocp_model.set('name','test_model_with_param');
ocp_model.set('T',T); %Refers to how much time simulate open-loop system
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_p',model.sym_p);
ocp_model.set('cost_type', 'nonlinear_ls');
ocp_model.set('cost_type_e', 'nonlinear_ls');
ocp_model.set('cost_expr_y', model.cost_expr_y);
%Below is the mayer term or terminal cost
ocp_model.set('cost_expr_y_e', model.cost_expr_y_e);
ocp_model.set('cost_W', model.W); %weighting matrices for lagrange term (in difference between reference and actual states)
ocp_model.set('cost_W_e', model.W_e); %weighting matrices for mayer term (in difference between reference and actual states)
%using explicit RK method
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', model.stateExpression);
%state and input constraints
ocp_model.set('constr_x0', x0);%initial state must be prescribed initial state
ocp_model.set('constr_Jbu',model.constr_Jbu); %next 3 lines describe constraints on control inputs
ocp_model.set('constr_lbu',model.constr_lbu);
ocp_model.set('constr_ubu',model.constr_ubu);
%Set characteristics of ocp operations
ocp_opts = acados_ocp_opts();
ocp_opts.set('param_scheme_N', N); %prediction horizon in context of MPC
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('sim_method', sim_method);
ocp_opts.set('qp_solver', qp_solver);
ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N);
ocp_opts.set('ext_fun_compile_flags', ''); % '-O2'
%Create OCP SOLVER (open-loop solver)
ocp = acados_ocp(ocp_model, ocp_opts);
x_traj_init = zeros(nx, N+1);
u_traj_init = zeros(nu, N);%as last state due to (N-1)th control input
%initialize reference trajectory
yref=zeros(5,1);
yref_e=zeros(4,1);
%%%%Note: with OCP you are giving reference trajectory across the full prediction horizon
%%%%Note: in MPC only the current reference as determined by tau is given.
plotYRef=zeros(1,N+1);
%Setting reference trajectory for the OCP problem
ocp.set('p',1);
for j=0:N-1
ocp.set('cost_y_ref',yref,j);%set reference trajectory across horizon (have reference for x and u)
end
ocp.set('cost_y_ref_e',yref_e,N);%set terminal state reference
%Solve and get results
ocp.solve();
utraj = ocp.get('u');
xtraj = ocp.get('x');
status = ocp.get('status'); % 0 - success
ocp.print('stat')
%%%%Plots
ts = linspace(0, T, N+1);
figure; hold on;
States = {'p', 'theta', 'v', 'dtheta'};
for i=1:length(States)
subplot(length(States), 1, i);
plot(ts, xtraj(i,:)); grid on; hold on;
plot(ts,plotYRef);
ylabel(States{i});
xlabel('t [s]')
end
%Plot control input trajectory
figure
stairs(ts, [utraj'; utraj(end)])
ylabel('F [N]')
xlabel('t [s]')
grid on
%%output position reference trajectory
referenceVariables=xtraj;%get position trajectory
tOCP=T;
time_Vect=linspace(0,tOCP,length(referenceVariables(1,:))); %must both OCP and MPC have same T and N such consistent timing
AND This is the model which both the OCP and integrator are based on.
function model= test_model_with_param()
%Import CasADi for non-linear modelling and algorithms differentiation
import casadi.*
%define number of states and number of controls
nx=4;
nu=1;
%Define Model parameters
M=SX.sym('M');
m=0.1;
l=0.8;
g=9.81;
%%%%%OCP model without tau as will not have reference control signal + just want to get reference trajectory
%Name states using scalar valued expression type in CasADi
%All units are in SI units
p=SX.sym('p'); %horizontal displacement of the cart
theta=SX.sym('theta');%angle between upwards vertical and rod (0 means upright)
v=SX.sym('v');%horizontal velocity of cart
dtheta=SX.sym('dtheta');%angular velocity of rod
F=SX.sym('F');%control input (force)
%State Vector and its derivative
sym_x=vertcat(p,theta,v,dtheta);%vertically concatenate state variables
sym_xdot=SX.sym('xdot',nx,1);%Create derivative of state vector
sym_u=F; %define control input as force
sym_p=M;
%describing dynamics of system
denom= M+m-m*cos(theta).^2;
stateExpression=vertcat(v, ...
dtheta, ...
(-m*l*sin(theta)*dtheta.^2+m*g*cos(theta)*sin(theta)+F)/denom, ...
(-m*l*cos(theta)*sin(theta)*dtheta.^2+F*cos(theta)+(M+m)*g*sin(theta))/l*denom ...
);
implicitStateExpression=stateExpression-sym_xdot;
%constraints
constr_Jbu=[1];
Umax=80;
constr_lbu=-Umax;
constr_ubu=Umax;%no slack variables in input constraint (hard constraint)
%Note: usually Zl should be larger
%cost function
W_x=diag([1e2 1e2 1e-3 1e-3]);%weight of each state in cost function
W_u=1e-1;
%cost function for reference tracking
cost_expr_y = vertcat(sym_x,sym_u); %%currently only position is a reference
W = blkdiag(W_x,W_u);
cost_expr_y_e=sym_x;
W_e=W_x; %why because to maintain periodicity of the generated path
%Defining model struct parameters
model.nx=nx; %%%
model.nu=nu; %%%
model.sym_x=sym_x; %%%
model.sym_xdot=sym_xdot; %%%
model.sym_u=sym_u; %%%
model.stateExpression=stateExpression; %%%
model.implicitStateExpression=implicitStateExpression; %%%
model.W_x=W_x; %weighting matrix of states in cost function (not used currently but actually equal to W)
model.W_e=W_e; %weighting matrix of terminal states in cost function
model.W=W;
model.cost_expr_y=cost_expr_y;
model.cost_expr_y_e=cost_expr_y_e;
model.constr_Jbu=constr_Jbu;
model.constr_lbu=constr_lbu;
model.constr_ubu=constr_ubu;
model.sym_p=sym_p;
end
