% Attempt to solve a simple problem of finding the optimal force input to minimize
% transport time of a mass, using the optimizer() function. This is an SCP (Sequential
% Convex Programming) problem.
S = 10; % [m] Path length
delta = 0.1; % [m] Spatial discretization
s = 0:delta:S; % [m] Discretized distances
N = length(s)-1; % Number of discrete points before the final point
Qf = 1000; % [J] Initial energy in battery
% *** YALMIP variables
Ekin = sdpvar(N+1,1);
SOC = sdpvar(N+1,1);
F = sdpvar(N,1);
v = sdpvar(N+1,1);
dtds = sdpvar(N+1,1);
% *** Constraints
constraints = [...
Ekin(2:N+1) == Ekin(1:N)+delta*F
SOC(2:N+1) == SOC(1:N)-delta/Qf*F
F(:) <= 10
F(:) >= -10
Ekin(:) >= 0.5*v(:).^2
Ekin(1) == 0.01
Ekin(N+1) == 0.01
SOC(1) == 1
cone([v.'+dtds.'
2*ones(1,N+1)
v.'-dtds.'])
];
% *** Objective function
cost = sum(dtds);
% *** YALMIP options
yalmip_opts = sdpsettings('verbose',1,'solver','gurobi');%'ecos','ecos.maxit',200);
% *** Solution
sol = optimize(constraints,cost,yalmip_opts);
% *** Extract duals of the first set of equality constraints (kinetic energy dynamics)
Ekin_duals = dual(constraints(1)); % PROBLEM: returns vectors of 100 NaN elements ?????