Initial Commit.

MILP_find_OptimalTraj_mpc_m3pi.m

@@ -0,0 +1,196 @@
+function argvout = MILP_find_OptimalTraj_mpc_m3pi(SP, plot_flag, hfig)
+% argvout = MILP_FIND_OPTIMALTRAJ_MPC_M3PI(SP, plot_flag, hfig) implements the
+% receding horizon controller for finding the optimal system trajectory
+
+%% Initialize variables
+
+tIndxs = 1:SP.pred_hor+1;
+x_cur = SP.x0;
+SP.u0.values = zeros(SP.n_inputs, SP.hor_length + 1);
+uvals = zeros(SP.n_inputs, SP.hor_length + 1);
+yvals = zeros(SP.n_outputs, SP.hor_length + 1);
+solutions{3} = zeros(SP.n_outputs, SP.input_length);
+SP.yout = zeros(SP.n_outputs, SP.input_length);
+
+SP.sol_count_vals = zeros;
+
+SP.states_system_prev = SP.states_system;
+SP.states_system_estimate = SP.states_system;
+
+unknown_pred_count = 0;
+
+%% Create unicycle agents for plotting and other plot variables initialization
+
+if plot_flag
+
+  figure(hfig);
+  car_sz = 0.05;
+  car = create_and_draw_car([SP.states_system(1:2);0], eye(3), 0 , 1, car_sz);
+  time_disp=text(0,0.5,0,'Time = 0.000 sec','Color','b', 'FontSize', 20);
+
+  trail_length = 10;
+  car = create_display_vars_car(car, trail_length);
+
+  SP.predicted_traj_handle = plot(SP.states_system(1), SP.states_system(2), ...
+    'b', 'LineWidth', 2);
+
+  if ~isempty(SP.movieName)
+    f = getframe;
+    [im,map] = rgb2ind(f.cdata,256,'nodither');
+  end
+end
+
+
+%% RUN MPC OPTIMIZATION
+
+sw = tic;
+
+for ii = 1:SP.hor_length+1
+
+  iter_time = tic;
+
+  sol_count = 1;
+  SP.tIndxs = tIndxs;
+
+  SP.states_system_prev = SP.states_system;
+
+  SP.yout(:,ii) = x_cur(1:SP.n_outputs);
+
+
+  %=============================================================================
+  % get new predicate online
+  %=============================================================================
+  if (strcmpi(get(hfig,'currentch'),'b')) && unknown_pred_count < SP.pred_unknown_sz
+
+    unknown_pred_count = unknown_pred_count + 1;
+
+    % plot the new predicate
+    Z = sdpvar(SP.n_outputs,1);
+    plot(SP.pred(SP.pred_known_sz+unknown_pred_count).A*Z <= SP.pred...
+      (SP.pred_known_sz+unknown_pred_count).b, [], [1 0 0]);
+    fill(SP.pred(SP.pred_known_sz+unknown_pred_count).values(:,1), ...
+      SP.pred(SP.pred_known_sz+unknown_pred_count).values(:,2) , [0.5 0 0]);
+
+    set(hfig,'currentch','p'); % set the character value to some different key
+
+    % update MTL specification
+    SP.phi = strcat(SP.phi, ' /\ ', SP.phi_global_unknown(unknown_pred_count).str);    
+  end
+
+  SP.dmin_con = -Inf;
+
+  %=============================================================================
+  % Solve for the new trajectory of the system
+  %=============================================================================
+
+  SP.tInd_con = 0;
+  SP.crit_pred = 0;
+  SP.constraints_tInd = zeros;
+  SP.crit_predVal = zeros;
+
+
+  SP.break_loop = 0;
+
+  prev_tInd_ind = [];
+
+
+  while((isempty(prev_tInd_ind) || (~isempty(prev_tInd_ind) && isempty(find...
+      (SP.crit_predVal(prev_tInd_ind) == SP.crit_pred, 1))))  && ~SP.break_loop && toc(iter_time) < 0.5*SP.ds)
+
+    SP.constraints_tInd(sol_count) = SP.tInd_con;
+    SP.crit_predVal(sol_count) = SP.crit_pred;
+
+    [solutions,diagnostics] = SP.controller{{x_cur(:) , SP.con_active , ...
+      (SP.hor_length + 1 - (ii-1)), SP.u0.dist_input_values, SP.pred.A, SP.pred.b}};
+
+
+    if diagnostics == 1 || diagnostics == 12 || diagnostics == 15
+      error('The problem is infeasible');
+    end
+
+    SP.u0.values = solutions{1};
+    SP.xout_disc = solutions{2}';
+    SP.yout(:,ii+1:end) = solutions{3}(:,2:end-(ii-1));
+
+
+    %===========================================================================
+    % Compute Robustness and identify the critical constraint
+    %===========================================================================
+
+
+    [~, SP.tmin_con, SP.dmin_con, ~, SP.crit_pred] = staliro_distance(SP, ...
+      SP.yout', SP.times');
+
+
+    if SP.tmin_con > ((ii-1)*SP.ds) && abs(SP.dmin_con) ...
+        > 1e-5 && sign(SP.dmin_con) == -1
+
+      % add constraints corresponding to the critical predicates
+      SP.tInd_con = round(SP.tmin_con/SP.ds)+1 - (ii-1);
+      SP.con_active(SP.crit_pred, SP.tInd_con) = 1;
+      prev_tInd_ind = find(SP.constraints_tInd == SP.tInd_con);
+
+    else
+
+      SP.break_loop = 1;
+
+    end
+
+    sol_count = sol_count + 1;
+
+  end
+
+  SP.sol_count_vals(ii) = sol_count-1;
+  uvals(:,ii) = solutions{1}(:,1);
+  yvals(:,ii) = solutions{3}(:,1);
+
+
+  if plot_flag
+
+    figure(hfig);
+    car.z = [x_cur(1:2);0];
+    car.th = atan2(x_cur(4), x_cur(3));
+    [car, time_disp] = update_car_paths(car, time_disp, ii-1, SP.ds, trail_length);
+
+    set(SP.predicted_traj_handle, 'XData', solutions{3}(1,1:size(tIndxs,2)), ...
+      'YData', solutions{3}(2,1:size(tIndxs,2)));
+
+    drawnow;
+
+    if ~isempty(SP.movieName)
+      f=getframe;
+      im(:,:,1,ii) = rgb2ind(f.cdata,map,'nodither');
+    end
+  end
+
+  tIndxs(1) = [];
+
+  x_cur = SP.xout_disc(SP.ctrl_hor+1,:)'; % update x0 for next iteration
+
+  SP.con_active = [zeros(size(SP.pred_orig,2),1) SP.con_active(:,3:end) ...
+    zeros(size(SP.pred_orig,2),1)];
+
+  pause(SP.ds - toc(iter_time))
+
+end
+
+
+sprintf('Average time taken to solve the optimization problem is %d in %d solutions', ...
+  toc(sw)/sum(SP.sol_count_vals), sum(SP.sol_count_vals))
+
+if ~isempty(SP.movieName)
+  imwrite(im,map, strcat(SP.movieName,'.gif'),'DelayTime', SP.ds,'LoopCount',inf);
+  argvout.im = im;
+  argvout.map = map;
+end
+
+%% Output results
+SP.u0.values = uvals;
+SP.yout = yvals;
+argvout.SP = SP;
+argvout.input = SP.u0;
+
+
+end
+
+

README.md

@@ -0,0 +1,23 @@
+# MILP-MTL
+An MILP Approach for Real-time Optimal Controller Synthesis with Metric Temporal Logic Specifications
+
+# Installing Dependencies
+
+This code depends on YALMIP, which can be obtained with the Multi-Parametric Toolbox, or MPT3, 
+see http://control.ee.ethz.ch/~mpt/3/Main/Installation.
+
+Computing robustness for the MTL specifications depends on the s-TaLiRo tool, which can be downloaded from
+https://sites.google.com/a/asu.edu/s-taliro/s-taliro/download
+
+We use the Gurobi solver as back-end to solve the optimization problem, though other solvers might work as well. 
+For the user-interactive example to work without modifications, Gurobi needs to be installed and configured for Matlab. 
+See http://www.gurobi.com.
+
+# Example
+
+Once everything is installed, you can run the file 'find_OptimalTraj_mpc_m3pi.m' and choose either to run the in-built 
+example or provide a reach-avoid MTL specification of your choice.
+
+# Contact Us
+
+You can contact sahas3@rpi.edu for any queries or to report any bugs in the code.

create_and_draw_car.m

@@ -0,0 +1,47 @@
+function obj = create_and_draw_car(z_n,R_n,mode,n, varargin)
+% CREATE_AND_DRAW_CAR - Creates image of object (car) to be controlled
+%   r - Position vector of object
+%   R - Rotation matrix of object
+%   mode - Called in CREATE_CAR
+
+%% Object : Car
+
+if nargin == 0
+    scale_size = 1;
+else
+    scale_size = varargin{1};
+end
+
+for j = 1:n
+
+    obj(j) = create_car(mode, scale_size);
+
+end
+
+for j = 1:n
+
+    r = z_n(1:3,j);
+    R = R_n(1:3,(j-1)*3+1:j*3);
+    % Position object according to state location and orientation
+    % and Plot the components
+
+    obj(j).Handle = patch('Faces',obj(j).facs,'Vertices',...
+        obj(j).vertices','FaceColor',obj(j).color,...
+        'FaceAlpha',obj(j).alpha);
+    % handle for the 3D model of the car
+
+    obj(j).vertices = obj(j).vertices + r(:,ones(1,length(obj(j).vertices)));
+    set(obj(j).Handle,'Vertices',obj(j).vertices');
+
+    for i = 1:4
+        coords = trans(obj(j).arm(i).wheels.coords,R,r);
+        obj(j).arm(i).wheels.plot = ...
+            fill3(coords(1,:), coords(2,:), coords(3,:), [0 0 1]);
+    end
+
+    obj(j).trajHandle = line(r(1,:), r(2,:), r(3,:), 'color', [.4 .4 .8],'LineWidth', 2);
+    % handle for the trajectory followed by the car
+
+    obj(j).z = z_n(:,j);
+
+end

create_display_vars_car.m

@@ -0,0 +1,15 @@
+function objsC = create_display_vars_car(objsC, lt)
+% CREATE_DISPLAY_VARS(objsC, lt) - creates variables for display features
+%    objsC - car objects which are to be displayed
+%      lt  - trail length of object paths
+
+
+cObjC = size(objsC,2);
+for i=1:cObjC
+    objsC(i).handle = plot3(objsC(i).z(1),objsC(i).z(2),objsC(i).z(3),'g--','LineWidth',2); % quad handle
+    objsC(i).disp_st = [objsC(i).z(1)*ones(lt,1) objsC(i).z(2)*ones(lt,1) objsC(i).z(3)*ones(lt,1)]; % Trail of the quads
+    objsC(i).ptr = 1;
+
+end
+
+end