minimum time to follow a path

[Mohamed Abdalla Mohamed Soliman]

Hello all,
If I have a path from point A to point B and at a certain point C I would like to solve an optimization problem (OCP) to calculate the minmium time from point C to point B taking into account the car dynamics
The OCP should accept the system states at point C. pathx and pathy are path coordinates as functions of artifical system dyanmics (z1,z2)
I would be thankful if someone can help.
%% Here we slove an MPC path follwoing problem, the positions at each time step is calculated online and passed to ACADO to follow using NMPC
clear all
BEGIN_ACADO; % Always start with "BEGIN_ACADO".

acadoSet('problemname', 'time_optimal_car');%

DifferentialState x y yaw velocity steer z1 z2 ;
Parameter T
Control u1 u2 v
%% Path coefficients

%% Car parameters
L = 0.162;
%% Systme dynamics
f = acado.DifferentialEquation(0.0, T);
f.add(dot(x) == velocitycos(yaw)); % Write down your ODE.
f.add(dot(y) == velocitysin(yaw)); %
f.add(dot(yaw) == velocity/L*tan(steer));
f.add(dot(velocity) == u1);
f.add(dot(steer) == u2);
f.add(dot(z1) == z2);
f.add(dot(z2) == v);

%% Optimal Control Problem
ocp = acado.OCP(0.0,T,50); % Set up the Optimal Control Problem (OCP)
% Start at 0 and go to T.
h={x,y};
r= [pathx,pathy];
Q = eye(2); % The weighting matrix
Q(1,1) = 10;
Q(2,2) = 10;

ocp.minimizeMayerTerm(T); % Minimize the consumed energy
ocp.minimizeLSQ( Q,h,r);

ocp.subjectTo( f ); % Optimize with respect to your differential equation

ocp.subjectTo( 'AT_START', x == 0.0 );
ocp.subjectTo( 'AT_START', y == 0.0 );
ocp.subjectTo( 'AT_START', yaw == pi/4 );
ocp.subjectTo( 'AT_START', velocity == 1.2 );
ocp.subjectTo( 'AT_START', steer == 0.0 );
ocp.subjectTo( 'AT_START', z1 == 0.0 );
ocp.subjectTo( 'AT_START', z2 == 0.0 );

% ocp.subjectTo( 'AT_END' , x == 10 );
% ocp.subjectTo( 'AT_END' , y == 10 );
ocp.subjectTo( 'AT_END' , z1 == 1 );

ocp.subjectTo( 'AT_END' , velocity == 0.0 );
% ocp.subjectTo( 'AT_END' , steer == 0.0 );

ocp.subjectTo( -0.2443 <= steer <= 0.2443 ); % path constraint on speed
ocp.subjectTo( 0.0 <= velocity <= 1.2 ); % Restrict the control, since otherwise it would take very large values in order to minimize T
ocp.subjectTo( -0.3 <= u1 <= 0.5 ); % Restrict the time horizon
ocp.subjectTo( -0.1 <= u2 <= 0.1 );
ocp.subjectTo( 0 <= x <= 100 );
ocp.subjectTo( 0 <= y <= 100 );

%%

algo =acado.OptimizationAlgorithm(ocp); % Set up the optimization algorithm, link it to your OCP
algo.set( 'KKT_TOLERANCE', 1e-10 ); % Set a custom KKT tolerance

END_ACADO; % Always end with "END_ACADO".
% This will generate a file problemname_ACADO.m.
% Run this file to get your results. You can
% run the file problemname_ACADO.m as many
% times as you want without having to compile again.

% Run the test
out = time_optimal_car_RUN();

BR
Soliman

[Mohamed Abdalla Mohamed Soliman]

Hello,
I tried this piece of code but this does not work
clear all;clc;

BEGIN_ACADO;

acadoSet('problemname', 'exp_check');

DifferentialState x y yaw velocity steer  ; 
Parameter T
Control u1 u2


input1 = acado.MexInputVector;       % x0 is free
input2 = acado.MexInputVector;
input3 = acado.MexInputVector;
input4 = acado.MexInputVector;
input5 = acado.MexInputVector;

input8 = acado.MexInputVector;       % x0 is free
input9 = acado.MexInputVector;
input10 = acado.MexInputVector;
input11 = acado.MexInputVector;
input12 = acado.MexInputVector;
input13 = acado.MexInputMatrix;


t_start = 0.0;
t_end   = T;

L = 0.162; 
% Differential Equation
f = acado.DifferentialEquation(0.0, t_end);
f.add(dot(x) == velocity*cos(yaw));                     % Write down your ODE. 
f.add(dot(y) == velocity*sin(yaw));        %
f.add(dot(yaw) == velocity/L*tan(steer)); 
f.add(dot(velocity) == u1); 
f.add(dot(steer) == u2);

%% Optimal Control Problem
ocp = acado.OCP(t_start, t_end, 50 ); 
ocp.minimizeMayerTerm(T);



ocp.subjectTo( f );

ocp.subjectTo( 'AT_START', x ==  input1 );     
ocp.subjectTo( 'AT_START', y ==  input2);    
ocp.subjectTo( 'AT_START', yaw ==  input3 );    
ocp.subjectTo( 'AT_START', velocity ==  input4 ); 
ocp.subjectTo( 'AT_START', steer    ==  input5 );


ocp.subjectTo( 'AT_END', x ==  input8 );     
ocp.subjectTo( 'AT_END', y ==  input9);    
ocp.subjectTo( 'AT_END', yaw ==  input10 );    
ocp.subjectTo( 'AT_END', velocity ==  input11 ); 
ocp.subjectTo( 'AT_END', steer    ==  input12 );


ocp.subjectTo( -0.2443 <= steer <= 0.2443 );% these values are given by the company R&D by email (14 deg)
ocp.subjectTo( 0.0 <= velocity   <= 1.200 );% Bounds
ocp.subjectTo( -0.3 <= u1 <= 0.5 );  % Acc
ocp.subjectTo( -0.1 <= u2   <= 0.1 );% steering rate
ocp.subjectTo( 0 <= x   <= 100 );
ocp.subjectTo( 0 <= y   <= 100 );% Bounds






% SETUP OF THE ALGORITHM AND THE TUNING OPTIONS:
algo = acado.OptimizationAlgorithm(ocp);
algo.set('KKT_TOLERANCE', 1e-4); 
algo.initializeControls(input13);






END_ACADO;

out = exp_check_RUN(5 ,5,pi/4,0,0, 15 ,15,pi/4,0,0, [0 0.0 0.0]) ;

[Mohamed Abdalla Mohamed Soliman]

I partially solved the problem by using
input1 = acado.MexInputVector;
input2 =acado.MexInputMatrix;
so I can do path following without code generation but still have one last problem which is how can I solve pf and optimal time ocp. At the moment when I use
ocp.minimizeMayerTerm(T);
h={x-pathx,y-pathy,z1};
I have an error
Any suggestions