function ObstacleAvoidance_splines()
% Example script for getting started with FORCESPRO NLP solver.
%
% --------------------------------------------------------------------------
% NOTE: This example shows how to define a nonlinear programming problem,
% where all derivative information is automatically generated by
% using the AD tool CasADi. In particular, this example illustrates the
% use of splines inside the problem formulation.
% --------------------------------------------------------------------------
%
% This example solves an optimization problem for a car with the simple
% continuous-time, nonlinear dynamics (bicycle model):
%
% dxPos/dt = v*cos(theta + beta)
% dyPos/dt = v*sin(theta + beta)
% dv/dt = F/m
% dtheta/dt = v/l_r*sin(beta)
% ddelta/dt = phi
%
% with:
% beta = arctan(l_r/(l_f + l_r)*tan(delta))
%
% where xPos,yPos are the position, v the velocity in heading angle theta
% of the car, and delta is the steering angle relative to the heading
% angle. The inputs are acceleration force F and steering rate phi. The
% physical constants m, l_r and l_f denote the car's mass and the distance
% from the car's center of gravity to the rear wheels and the front wheels.
%
% The car starts from standstill with a certain heading angle, and the
% optimization problem is to move the car as close as possible to a certain
% end position while staying inside a nonconvex area. In addition, an
% elliptic obstacle must be avoided. Using an interactive window,
% the position of the obstacle can be changed by the user and the car's
% trajectory is adapted automatically.
%
% Quadratic costs for the acceleration force and steering are added to the
% objective to avoid excessive maneouvers.
%
% There are bounds on all variables except theta.
%
% Variables are collected stage-wise into
%
% z = [F phi xPos yPos v theta delta].
%
% See also FORCES_NLP
%
% (c) Embotech AG, Zurich, Switzerland, 2013-2023.
clc;
close all;
%% Generate code for path finder
model = generatePathplanner();
%% Calculate path for fixed obstacle
% Set runtime parameters
params = [-1.5; 1.0];
problem.all_parameters = repmat(params,model.N,1);
% Set initial guess to start solver from (here, middle of upper and lower
% bound)
x0i=[0.0,0.0,-1.5,1.5,1.,pi/4.,0.];
x0=repmat(x0i',model.N,1);
problem.x0=x0;
% Set initial conditions. This is usually changing from problem instance
% to problem instance:
model.xinit = [-2., 0., 0., deg2rad(90), 0.]';
problem.xinit = model.xinit; % xPos=-2, yPos=0, v=0 (standstill),
% heading angle=90deg, steering angle=0deg
% Time to solve the NLP!
[output,exitflag,info] = FORCESNLPsolver(problem);
% Make sure the solver has exited properly.
assert(exitflag == 1,'Some problem in FORCESPRO solver');
% extract output
u = zeros(2,model.N);
x = zeros(5,model.N);
for i=1:model.N
temp = output.(['x',sprintf('%02d',i)]);
x(:,i) = temp(3:7,1);
u(:,i) = temp(1:2,1);
end
%% Plot results and make window interactive
plotAndMakeInteractive(x,u,model,params);
end
function [model] = generatePathplanner()
% Formulates the optimization problem, generates a solver for the path
% planning problem by calling the FORCESPRO code generation.
%% Problem dimensions
model.N = 50; % horizon length
model.nvar = 7; % number of variables
model.neq = 5; % number of equality constraints
model.nh = 3; % number of inequality constraint functions
model.npar = 2; % number of runtime parameters
%% Objective function
% In this example, we want to move the car as close as possible to the
% point (0,3) while adding some penalties on the inputs F and s:
model.objective = @(z) 100*abs(z(3)-0) + 100*abs(z(4)-3.) + ...
0.1*z(1)^2 + 0.01*z(2)^2;
% You can use standard Matlab handles to define these functions, i.e.
% you could also have this in a separate file. We use anonymous handles
% here only for convenience.
%% Dynamics, i.e. equality constraints
% We use an explicit RK4 integrator here to discretize continuous
% dynamics. The definition of @continuousDynamics is further below in
% this file
integrator_stepsize = 0.1;
model.eq = @(z) RK4( z(3:7), z(1:2), @continuousDynamics,...
integrator_stepsize);
% Indices on LHS of dynamical constraint - for efficiency reasons, make
% sure the matrix E has structure [0 I] where I is the identity matrix.
model.E = [zeros(5,2), eye(5)];
%% Inequality constraints
% upper/lower variable bounds lb <= z <= ub
% inputs | states
% F phi xPos yPos v theta delta
model.lb = [ -5.0, deg2rad(-40), -3., 0., 0., -inf, -0.48*pi];
model.ub = [ +5.0, deg2rad(+40), 0., 3., 2., +inf, 0.48*pi];
% General (differentiable) nonlinear inequalities hl <= h(z,p) <= hu
% using two splines for road limit and a circle for an additional
% obstacle
[upperRoadLimit, lowerRoadLimit] = setupRoadLimitSplines();
model.ineq = @(z,p) [ z(4) - upperRoadLimit(z(3)); ...
lowerRoadLimit(z(3)) - z(4); ...
(z(3)-p(1))^2 + (z(4)-p(2))^2 ];
% Upper/lower bounds for inequalities
model.hu = [ 0, 0, +inf ]';
model.hl = [ -inf, -inf, 0.6^2 ]';
%% Initial conditions
% Initial conditions on all states
model.xinitidx = 3:7; % use this to specify on which variables initial
% conditions are imposed
%% Define solver options
codeoptions = getOptions('FORCESNLPsolver');
codeoptions.maxit = 400; % Maximum number of iterations
codeoptions.printlevel = 0; % Use printlevel = 2 to print progress (but
% not for timings)
codeoptions.optlevel = 3; % 0: no optimization, 1: optimize for size,
% 2: optimize for speed, 3: optimize for size & speed
codeoptions.cleanup = false;
codeoptions.timing = 1;
codeoptions.nlp.bfgs_init = 3.0*eye(7); % set initialization of the
% hessian approximation
% change this to your server or leave uncommented for using the
% standard embotech server at https://www.embotech.com/codegen
% codeoptions.server = 'https://yourforcesserver.com:1234';
codeoptions.nlp.ad_tool = 'casadi';
%% Generate FORCESPRO solver
FORCES_NLP(model, codeoptions);
end
function rad = deg2rad(deg) % convert degrees into radians
rad = deg/180*pi;
end
function deg = rad2deg(rad) % convert radians into degrees
deg = rad/pi*180;
end
%% Callback for plot
function calculatePath(hObject,~,model)
% Run solver with the new clicked obstacle position and update the plots
% read in parameters
pos=get(gca,'CurrentPoint');
param1=pos(1,1);
param2=pos(1,2);
fprintf([newline 'You clicked X: %4.4f, Y: %4.4f'],param1,param2);
if ((param1 - model.xinit(1))^2 + (param2 - model.xinit(2))^2 ...
<= model.hl(3))
fprintf([newline ...
'The initial position cannot lie within obstacle. ' newline]);
elseif (param1 < model.lb(3) || param1 > model.ub(3) || ...
param2 < model.lb(4) || param2 > model.ub(4))
fprintf([newline 'The obstacle position you clicked is out of '...
'bounds' newline]);
else
% Set runtime parameters
params = [param1; param2];
problem.all_parameters = repmat(params,model.N,1);
% Set initial guess to start solver from:
x0i=[0.0,0.0,-1.5,1.5,1.,pi/4.,0.];
x0=repmat(x0i',model.N,1);
problem.x0=x0;
% Set initial conditions. This is usually changing from problem
% instance to problem instance
problem.xinit = model.xinit;
% Time to solve the NLP!
[output,exitflag,info] = FORCESNLPsolver(problem);
% Make sure the solver has exited properly.
if (exitflag < 0)
fprintf([newline 'Some error in FORCESPRO solver. exitflag=%d'...
newline],exitflag)
elseif (exitflag == 0)
fprintf([newline 'Error in FORCESPRO solver: Maximum number'...
' of iterations was reached. ' newline]);
else
fprintf([newline 'FORCESPRO took %d iterations and %f seconds '...
'to solve the problem.' newline], info.it, info.solvetime);
% extract the output
temp = zeros(model.nvar,model.N);
for i=1:model.N
temp(:,i) = output.(['x',sprintf('%02d',i)]);
end
u = temp(1:2,:);
x = temp(3:7,:);
% Update the plot with the new results
updatePlots(x,u,model,params);
end
end
end
function [xDot] = continuousDynamics(x,u)
% state x = [xPos,yPos,v,theta,delta], input u = [F, phi]
% set physical constants
l_r = 0.5; % distance rear wheels to center of gravity of the car
l_f = 0.5; % distance front wheels to center of gravity of the car
m = 1.0; % mass of the car
% set parameters
beta = atan(l_r/(l_f + l_r) * tan(x(5)));
% calculate dx/dt
xDot = [ x(3) * cos(x(4) + beta); % dxPos/dt = v*cos(theta+beta)
x(3) * sin(x(4) + beta); % dyPos/dt = v*cos(theta+beta)
u(1)/m; % dv/dt = F/m
x(3)/l_r * sin(beta); % dtheta/dt = v/l_r*sin(beta)
u(2)]; % ddelta/dt = phi
end
function [upperLimitSpline, lowerLimitSpline] = setupRoadLimitSplines()
% Returns two cubic splines representing the upper and lower limit
% of the road.
xPos = [-3.0, -2.0, -1.0, 0.0];
yUpper = [ 0.0, 1.8, 2.5, 3.0];
yLower = [-5.0, -2.0, 0.3, 1.5];
upperLimitSpline = ForcesInterpolationFit( xPos,yUpper );
lowerLimitSpline = ForcesInterpolationFit( xPos,yLower );
end
function updatePlots(x,u,model,params)
% Deletes old data sets in the current plot and adds the new data sets
% given by the arguments x, u and params to the plot.
% x: matrix consisting of a set of state column vectors
% u: matrix consisting of a set of input column vectors
% params: position of obstacle
% model: model struct required for the code generation of FORCESPRO
gcf();
% plot updated acceleration force
subplot(5,2,8);
oldAcc = findobj(gca,'Color','blue');
delete(oldAcc);
stairs(u(1,:),'b-'); grid on;
% plot updated steering rate
subplot(5,2,10)
oldPhi = findobj(gca,'Color','blue');
delete(oldPhi);
stairs(rad2deg(u(2,:)),'b-'); grid on;
% plot updated velocity
subplot(5,2,2)
oldVel = findobj(gca,'Color','blue');
delete(oldVel);
plot(x(3,:),'b-'); grid on;
% plot updated heading angle
subplot(5,2,4)
oldTheta = findobj(gca,'Color','blue');
delete(oldTheta)
plot(rad2deg(x(4,:)),'b-'); grid on;
% plot updated steering angle
subplot(5,2,6)
oldDelta = findobj(gca,'Color','blue');
delete(oldDelta);
plot(rad2deg(x(5,:)), 'b-'); grid on
% plot updated trajectory
subplot(5,2,[1,3,5,7,9])
oldPath = findobj(gca,'Color','blue','LineStyle','-');
delete(oldPath);
plot(x(1,:),x(2,:),'b-'); hold on;
oldObstacle = findobj(gca,'LineStyle','-.');
delete(oldObstacle);
rectangle('Position',[params(1)-...
sqrt(model.hl(3)) params(2)-sqrt(model.hl(3)) ...
2*sqrt(model.hl(3)) 2*sqrt(model.hl(3))],...
'Curvature',[1 1],'EdgeColor','r','LineStyle','-.');
legend('init pos','trajectory','Location','northwest');
end
function plotAndMakeInteractive(x,u,model,params)
% Creates a plot, adds the data sets provided by the arguments x, u, and
% params to the plot and makes the plot interactive by connecting a
% callback function
% x: matrix consisting of a set of state column vectors
% u: matrix consisting of a set of input column vectors
% params: position of obstacle
% model: model struct required for the code generation of FORCESPRO
figure(1); clf;
% plot acceleration force
subplot(5,2,8)
title('Acceleration force'); hold on;
plot([1 model.N], [model.ub(1) model.ub(1)]', 'r:');
plot([1 model.N], [model.lb(1) model.lb(1)]', 'r:');
stairs(u(1,:),'b-');
% plot steering rate
subplot(5,2,10);
title('Steering rate'); hold on;
plot([1 model.N], rad2deg([model.ub(2) model.ub(2)]'), 'r:');
plot([1 model.N], rad2deg([model.lb(2) model.lb(2)]'), 'r:');
stairs(rad2deg(u(2,:)),'b-');
% plot velocity
subplot(5,2,2)
title('Velocity'); hold on;
plot([1 model.N], [model.ub(5) model.ub(5)]', 'r:');
plot([1 model.N], [model.lb(5) model.lb(5)]', 'r:');
plot(x(3,:),'b-');
% plot heading angle
subplot(5,2,4)
title('Heading angle'); hold on;
xlim([1, model.N]);
ylim([-90,180]);
plot(rad2deg(x(4,:)),'b-');
% plot steering angle
subplot(5,2,6)
title('Steering angle'); hold on;
plot([1 model.N], rad2deg([model.ub(7) model.ub(7)])', 'r:');
plot([1 model.N], rad2deg([model.lb(7) model.lb(7)])', 'r:');
plot(rad2deg(x(5,:)),'b-');
% plot trajectory in interactive window
subplot(5,2,[1,3,7,9])
plot(model.xinit(1),model.xinit(2),'bx','LineWidth',3); hold on;
% plot road limits
[upperRoadLimit, lowerRoadLimit] = setupRoadLimitSplines();
[xStart, xEnd] = getRange(upperRoadLimit);
xPosFine = linspace( xStart,xEnd,100 );
upperRoadLimitFine = upperRoadLimit( xPosFine );
lowerRoadLimitFine = lowerRoadLimit( xPosFine );
plot( xPosFine,upperRoadLimitFine, 'r','LineStyle',':' );
plot( xPosFine,lowerRoadLimitFine, 'r','LineStyle',':' );
plot(x(1,:),x(2,:),'b-');
rectangle('Position',[params(1,1)- sqrt(model.hl(3))...
params(2,1)-sqrt(model.hl(3)) 2*sqrt(model.hl(3)) ...
2*sqrt(model.hl(3))],'Curvature',[1 1],'EdgeColor','r',...
'LineStyle','-.');
legend('init pos','trajectory','Location','northwest');
title(sprintf('Click into figure to place obstacle'));
axis equal
xlim([model.lb(3), model.ub(3)]);
ylim([model.lb(4),model.ub(4)]);
xlabel('x-coordinate'); ylabel('y-coordinate');
set(gcf,'WindowButtonDownFcn',{@calculatePath,model})
end