6.3. High-level interface: Obstacle avoidance

In this example we illustrate the simplicity of the high-level user interface on a vehicle optimal trajectory generation problem. In particular, we use a simple vehicle model described by a set of ordinary differential equations (ODEs):

\[\begin{split}\dot{x} & = v \cos(\theta) \\ \dot{y} & = v \sin(\theta) \\ \dot{v} & = F / m \\ \dot{\theta} & = s / L\end{split}\]

The model consists of four differential states: \(x\) and \(y\) are the Cartesian coordinates of the car, \(v\) is the linear velocity and \(\theta\) is the heading angle. Next, there are two control inputs to the model: the acceleration force \(F\) and the steering torque \(s\). The two parameters are the car mass \(m=1\,\mathrm{kg}\) and the wheel base which we assume to be \(L=1\,\mathrm{m}\).

The trajectory of the vehicle will be defined as an NLP. First, we define stage variable \(z\) by stacking the input and differential state variables:

\[z = [F,s,x,y,v,\theta]^\top\]

You can download the Matlab code of this example to try it out for yourself by clicking here.

6.3.1. Defining the problem data

6.3.1.1. Objective

In this example the cost function is the same for all stages. We want to maximize progress in the \(y\) direction, with quadratic penalties on the inputs \(F\) and \(s\), i.e.:

\[f(z) = -100 z_4 + 0.1 z_1^2 + 0.01 z_2^2\]

The stage cost function is coded in MATLAB as the following function:

function f = objective( z )
    F = z(1);
    s = z(2);
    y = z(4);
    f = -100*y + 0.1*F^2 + 0.01*s^2;
end

6.3.1.2. Matrix equality constraints

The matrix equality constraints in this example represent only the discretized dynamic equations of the vehicle using an explicit Runge-Kutta integrator of order 4. The vehicle dynamics defi ned above are represented by a function continuous_dynamics and the NLP constraint function \(c(\cdot)\) as the function dynamics. Note that the function RK4 is included in the FORCES Pro client software.

function xdot = continuous_dynamics(x, u)
    F = u(1);
    s = u(2);
    v = x(3);
    theta = x(4);
    m = 1;
    L = 1;
    xdot = [v * cos(theta);
            v * sin(theta);
            F / m;
            s / L];
end

function xnext = dynamics(z)
    x = z(3:6);
    u = z(1:2);
    % implements a RK4 integrator for the dynamics
    integrator_stepsize = 0.1;
    xnext = RK4(x, u, @continuous_dynamics, integrator_stepsize);
end

6.3.1.3. Inequality constraints

The maneuver is subjected to a set of constraints, involving both the simple bounds:

\begin{align*} -5\,\mathrm{N} \leq & F \leq 5\,\mathrm{N} \\ -1\,\mathrm{Nm} \leq & s \leq 1\,\mathrm{Nm} \\ -3\,\mathrm{m} \leq & x \leq 0\,\mathrm{m} \\ 0\,\mathrm{m} \leq & y \leq 3\,\mathrm{m} \\ 0\,\mathrm{m/s} \leq & v \leq 2\,\mathrm{m/s} \\ 0\,\mathrm{rad} \leq & \theta \leq \pi\,\mathrm{rad} \end{align*}

as well the nonlinear nonconvex constraints:

\begin{align*} 1\,\mathrm{m^2} \leq & x^2+y^2 \leq 9\,\mathrm{m^2} \\ 0.9025\,\mathrm{m^2} \leq & (x+2)^2 + (y-2.5)^2 \\ \end{align*}

In this case, the nonlinear constraint function \(h(\cdot)\) can be coded in MATLAB as follows:

function h = inequalities(z)
    x = z(3);
    y = z(4);
    h = [x^2 + y^2;
        (x +2)^2 + (y -2.5)^2 ];
end

6.3.1.4. Initial and final conditions

The goal of the maneuver is to steer the vehicle from a set of initial conditions:

\[x_{\mathrm{init}} = -2\,\mathrm{m} ,\quad y_{\mathrm{init}} = 0\,\mathrm{m} ,\quad v_{\mathrm{init}} = 0\,\mathrm{m/s} ,\quad \theta_{\mathrm{init}} = 2.0944\,\mathrm{rad}\]

to another point in the state-space subjected to the final conditions:

\[v_{\mathrm{final}} = 0\,\mathrm{m/s} ,\quad \theta_{\mathrm{final}} = 0\,\mathrm{rad}\]

6.3.2. Defining the MPC problem

With the above de fined MALTAB functions for objective, matrix equality and inequality functions, we can completely define the NLP formulation in the next code snippet. For this example, we chose to use \(N = 50\) stages in the NLP:

%% Problem dimensions
model.N = 50;            % horizon length
model.nvar = 6;          % number of variables
model.neq  = 4;          % number of equality constraints
model.nh = 2;            % number of inequality constraint functions

%% Objective function
model.objective = @objective;

%% Matrix equality constraints
model.eq = @dynamics;
model.E = [zeros(4, 2), eye( 4 )];

%% Inequality constraints
% upper/lower bounds lb <= z <= ub
model.lb = [-5, -1, -3, 0, 0, 0 ];
model.ub = [+5, +1, 0, 3, 2, +pi];

% Nonlinear inequalities hl <= h(z) <= hu
model.ineq = @inequalities;
model.hu = [9, +inf]';
model.hl = [1, 0.95^2]';

%% Initial and final conditions
model.xinit = [-2, 0, 0, deg2rad(120)]';
model.xinitidx = 3:6;
model.xfinal = [0, deg2rad(0)]';
model.xfinalidx = 5:6;

6.3.3. Generating a solver

We have now populated model with the necessary fields to generate a solver for our problem. Now we set some options for our solver and then use the function FORCES_NLP to generate a solver for the problem defined by model with the first state as a parameter:

%% Define solver options
codeoptions = getOptions('FORCESNLPsolver');
codeoptions.maxit = 200;    % Maximum number of iterations
codeoptions.printlevel = 2; % Use printlevel = 2 to print progress (but not for timings)
codeoptions.optlevel = 0;   % 0: no optimization, 1: optimize for size, 2: optimize for speed, 3: optimize for size & speed
codeoptions.cleanup = false;
codeoptions.timing = 1;
codeoptions.printlevel = 0;

%% Generate forces solver
FORCES_NLP(model, codeoptions);

6.3.4. Calling the generated solver

Once all parameters have been populated, the MEX interface of the solver can be used to invoke it:

%% Call solver
% Set initial guess to start solver from:
x0i=model.lb+(model.ub-model.lb)/2;
x0=repmat(x0i',model.N,1);
problem.x0=x0;

% Set initial and final conditions. This is usually changing from problem
% instance to problem instance:
problem.xinit = model.xinit;
problem.xfinal = model.xfinal;

% Time to solve the NLP!
[output,exitflag,info] = FORCESNLPsolver(problem);

% Make sure the solver has exited properly.
assert(exitflag == 1,'Some problem in FORCES solver');
fprintf('\nFORCES took %d iterations and %f seconds to solve the problem.\n',info.it,info.solvetime);

6.3.5. Results

The goal is to find a trajectory that steers the vehicle from point A to another standstill point while avoiding obstacles and maximizing the progress on the y-direction along the way. The trajectory should also be feasible with respect to the vehicle dynamics and its safety and physical limitations. The vehicle’s trajectory in 2D space is presented in Figure 6.3.

The vehicle’s velocity and steering angle over time is presented in Figure 6.4, and the actuator commands in Figure 6.5. One can see that all constraints are respected.

../../_images/high_level_obstacle_avoidance_plot1.png

Figure 6.3 Vehicle’s trajectory in 2D space.

../../_images/high_level_obstacle_avoidance_plot2.png

Figure 6.4 Vehicle’s velocity and steering angle over time.

../../_images/high_level_obstacle_avoidance_plot3.png

Figure 6.5 Vehicle’s actuator commands over time.

6.3.6. Variation 1: Parameters

One possible variation is if we consider the mass \(m\) and wheel base \(L\) as parameters, so that we can tune them after the code generation. First we define the number of parameters:

for i=1:model.N-1
    model.npar(i) = 2; % number of parameters
end
model.npar(model.N) = 0; % no parameters in the last stage

and then include them into our dynamics function handles:

function xdot = continuous_dynamics(x, u, p)
    F = u(1);
    s = u(2);
    v = x(3);
    theta = x(4);
    m = p(1);
    L = p(2);
    xdot = [v * cos(theta);
            v * sin(theta);
            F / m;
            s / L];
end

function xnext = dynamics(z, p)
    x = z(3:6);
    u = z(1:2);
    % implements a RK4 integrator for the dynamics
    integrator_stepsize = 0.1;
    xnext = RK4(x, u, @continuous_dynamics, integrator_stepsize, p);
end

Note that we have to provide these parameters through the problem struct before calling the generated solver:

% Set parameters
problem.all_parameters = repmat([1 1]',model.N-1,1);

You can download the Matlab code of this variation to try it out for yourself by clicking here.

6.3.7. Variation 2: Different integrator

Another possible variation is if we want to change the integrator that is used to discretize the continuous-time dynamics. To do that we set, for example, the codeoptions.nlp fields:

% define integrator options
codeoptions.nlp.integrator.type = 'IRK4'; % can also be 'ForwardEuler', 'ERK2', 'ERK3', 'ERK4', 'BackwardEuler', or 'IRK2'
codeoptions.nlp.integrator.Ts = 0.1;
codeoptions.nlp.integrator.nodes = 10;

For more information regarding the different integrators available, see Integrators.

You can download the Matlab code of this variation to try it out for yourself by clicking here.

6.3.8. Variation 3: Terminal cost

Another possible variation is if we want to have a terminal cost that is different than the stage costs of the horizon. To do that we provide each cost function handle in a cell array as follows:

%% Objective function
% In this example, we want to penalize the inputs F and s:
for i=1:model.N-1
    model.objective{i} = @(z) 0.1*z(1)^2 + 0.01*z(2)^2;
end

% and maximize the progress on the y direction, while ensuring a small
% velocity and heading angle at the end of the horizon.
% Terminal cost: -100*y 100*v^2 + 100*theta^2 to aim for max y, v=0 and theta=0
model.objective{model.N} = @(z) -100*z(4) + 10*(z(5)-0)^2 + 10*(z(6)-0)^2;

You can download the Matlab code of this variation to try it out for yourself by clicking here.

6.3.9. Variation 4: External functions

One final variation is if we supply the required functions through external functions in C. To do that we have to provide the directory that contains said source files:

%% Define source file containing function evaluation code
model.extfuncs = 'C/myfevals.c';

We also need to include the two extern functions car_dyanmics and car_dyanmics_jacobian, both contained in the car_dynamics.c file, through the other_srcs options field:

% add additional source files required - separate by spaces if more than 1
codeoptions.nlp.other_srcs = 'C/car_dynamics.c';

You can download the Matlab code of this variation to try it out for yourself by clicking here.