I also want to solving NLOCP with fixed sampling time using multiple shooting method and opti stack, where x, u and N are my decision variables.
%Import
%% Time setting
Ts = 1e-1; % sampling time
%% Initialization
mc = 1; % mass of the cart [kg]
mp = 0.2; % mass of the pendulum [kg]
l = 0.5; % length pendulum [m]
fc = 0.5; % friction coefficient cart [Ns/m]
fp = 1e-4; % friction coefficient pendulum [Nms/rad]
g = 9.81; % gravity acceleration [m/s^2]
param = [mc, mp, l, fc, fp, g]; % parameter vector
x0 = [0,0,0,0]'; % initial condition [x;x_dot;phi;phi_dot]
xs = [0,0,pi,0]'; % Reference state
%% Time integration method
f = load('./Fcns/pendulum_on_cart.mat').f; % nonlinear function f(x,u)
n_states = size(f.mx_in{1},1); % dimension: state vector
n_ctrl = size(f.mx_in{2},1); % dimension: actuator vector
% Integrator
intg_options = struct;
intg_options.number_of_finite_elements = 4;
% DAE
x = MX.sym('x',n_states);
u = MX.sym('u',n_ctrl);
dae.x = x;
dae.p = u;
dae.ode = f(x,u,param);
intg = integrator('intg','rk',dae,intg_options);
res = intg('x0',x,'p',u);
x_next = res.xf;
F_mpc = Function('F_mpc',{x,u},{x_next},{'x','u'},{'x_next'});
%% OCP Mulitiple-shooting
Nmax = 30;
opti = casadi.Opti();
X = opti.variable(n_states,Nmax+1); % state trajectory
U = opti.variable(n_ctrl,Nmax); % control trajectory
N = opti.variable(); % final time
xk = opti.parameter(n_states,1);
xf = opti.parameter(n_states,1);
opti.minimize(N); % Objective function
% Dynamic
for k = 1:Nmax
opti.subject_to(X(:,k+1)==F_mpc(X(:,k),U(:,k)));
end
% Path constraints
x_max = 0.5; % maximum position of cart [m]
x_min = -x_max; % minimum position of cart [m]
opti.subject_to(x_min<=X(1,:)<=x_max);
v_max = 3; % maximum velocity of cart [m/s]
v_min = -v_max; % minimum velocity of cart [m/s]
opti.subject_to(v_min<=X(2,:)<=v_max);
alpha_max = inf; % maximum angle of pendulum [rad]
alpha_min = -alpha_max; % maximum angle of pendulum [rad]
opti.subject_to(alpha_min<=X(3,:)<=alpha_max);
omega_max = 3*pi; % maximum angular velocity of pendulum [rad/s]
omega_min = -omega_max; % maximum angular velocity of pendulum [rad/s]
opti.subject_to(omega_min<=X(4,:)<=omega_max);
% Input constraints
f_max = 1e2; % maximum input force [N]
f_min = -f_max; % minimum input force [N]
opti.subject_to(f_min<=U<=f_max);
% Boundary conditions
opti.subject_to(X(:,1)== xk); % initial condition
opti.subject_to(X(:,N)== xf); % final state
% Time constrians
opti.subject_to(0<=N<Nmax); % Time must be positive
% Solver
opts = struct; % options
opts.discrete = [false,false,true];
opti.solver('bonmin',opts);
opti.set_initial(N , 0); % initial guesses for the solver
opti.set_value(xk,x0);
opti.set_value(xf,xs);
try
sol = opti.solve();
catch ERR
ERR.message
end