function [] = ObstacleAvoidance_ownFevals() % Example script for getting started with FORCESPRO NLP solver. % % -------------------------------------------------------------------------- % NOTE: This example shows how to pass C functions implementing the % function evaluations to FORCES. There is an automated way of creating % these C functions, which is explained in the other example file named % "ObstacleAvoidance.m" % -------------------------------------------------------------------------- % % 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=90°, steering angle=0° % Time to solve the NLP! [output,exitflag,info] = FORCESNLPsolver(problem); %#ok % 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 = 2; % number of inequality constraint functions model.npar = 2; % number of runtime parameters %% Define source file containing function evaluation code model.extfuncs = 'C/myfevals.c'; % Note: we will add additional source files required below at code % options! % 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 % model.ineq = @(z,p) [ z(3)^2 + z(4)^2; % (z(3)-p(1))^2 + (z(4)-p(2))^2 ]; % Upper/lower bounds for inequalities model.hu = [9, +inf]'; model.hl = [1, 0.7^2]'; %% Initial conditions % Initial conditions on all states model.xinitidx = 3:7; % use this to specify on which variables initial % conditions are imposed model.bfgs_init = 3.000 * eye(7); % set initialization of the % hessian approximation %% Define solver options codeoptions = getOptions('FORCESNLPsolver'); codeoptions.maxit = 400; % 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; % 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.other_srcs = 'C/car_dynamics.c'; %% 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(~,~,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(2)) 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 [] = 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(2)) params(2)-sqrt(model.hl(2)) ... 2*sqrt(model.hl(2)) 2*sqrt(model.hl(2))],... '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; rectangle('Position',[-sqrt(model.hl(1)) -sqrt(model.hl(1)) ... 2*sqrt(model.hl(1)) 2*sqrt(model.hl(1))],'Curvature',[1 1], ... 'EdgeColor','r','LineStyle',':'); rectangle('Position',[-sqrt(model.hu(1)) -sqrt(model.hu(1)) ... 2*sqrt(model.hu(1)) 2*sqrt(model.hu(1))],'Curvature',[1 1], ... 'EdgeColor','r','LineStyle',':'); plot(x(1,:),x(2,:),'b-'); rectangle('Position',[params(1,1)- sqrt(model.hl(2))... params(2,1)-sqrt(model.hl(2)) 2*sqrt(model.hl(2)) ... 2*sqrt(model.hl(2))],'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