diff --git a/MILP_find_OptimalTraj_mpc_m3pi.m b/MILP_find_OptimalTraj_mpc_m3pi.m new file mode 100644 index 0000000..0d6d2f6 --- /dev/null +++ b/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 + + diff --git a/README.md b/README.md new file mode 100644 index 0000000..b720219 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/create_and_draw_car.m b/create_and_draw_car.m new file mode 100644 index 0000000..9e8e08b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/create_display_vars_car.m b/create_display_vars_car.m new file mode 100644 index 0000000..7cf8929 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/find_OptimalTraj_mpc_m3pi.m b/find_OptimalTraj_mpc_m3pi.m new file mode 100644 index 0000000..beb56d3 --- /dev/null +++ b/find_OptimalTraj_mpc_m3pi.m @@ -0,0 +1,167 @@ +function argvout = find_OptimalTraj_mpc_m3pi() +% argvout = FIND_OPTIMALTRAJ_MPC_M3PI() sets up the problem of finding the +% optimal trajectory for a m3pi robot with unicycle dynamics, by minimizing +% the 1-norm of the control input over the prediction horizon using a +% receding horizon framework such that the resultant trajectory satisfies a +% given Metric Temporal Logic (MTL) specification with a desired robustness. + +dbstop if error; +close all; + +userInput = input('Type "yes" to run the user-interactive example. ', 's'); +movieName = input('Type in the file name if you want to make a movie in .gif format. ', 's'); + +%% Set Variable Parameters + +% set what is to be optimized +obj_u = 1; % when set minimizes 1-norm of control signal + + +ctrl_horizon = 1; + +rob_ball_des = 0.05; + +plot_flag = 1; % when set plots the position of the m3pi robot in each iteration + +ds = 0.5; % time-step for running simulation + +%% Describe system + +SP = get_system_description(); +SP.t0 = 0; +SP.ds = ds; + +SP.pred_known_sz = []; +SP.pred_unknown_sz = []; +SP.pred_unknown_sz_orig = SP.pred_unknown_sz; +SP.unknown_pred_flag = 0; + +SP = select_Predicates(SP, userInput); +hfig = SP.hfig; + +if isempty(SP.pred_unknown_sz) + SP.pred_num = SP.pred_known_sz; +else + SP.pred_num = SP.pred_known_sz + SP.pred_unknown_sz; +end + +SP.rad = 0.067/2; % size of the m3pi robot + + +%% -------------------------- System Parameters -------------------------------- + + +SP.times = SP.t0:SP.ds:SP.tf; +SP.u0.times = SP.times; + +SP.input_length = size(SP.times,2); + +SP.return_flag = 0; + +SP.u0.dist_input_values = SP.times*0.; +SP.u0.dist_input_values = SP.u0.dist_input_values(ones(SP.n_outputs,1),:); +SP.u0.dist_input_values = 0.0*rand(size(SP.u0.dist_input_values)); + + +SP.u0.input_values_human = zeros(SP.n_inputs, SP.input_length); + +% Initialize state variables + +SP.states_system = [SP.x0(1:2); 0]; % [x-pos; y-pos; theta] +SP.controller_input_system = [0 0]'; % control input to the nonlinear + % system dynamics + + +%% ------------------------- Optimization Parameters --------------------------- + +SP.rob_ball_des = rob_ball_des; +SP.obj_u = obj_u; +SP.ctrl_hor = ctrl_horizon; +SP.hor_length = SP.pred_hor; % simulation horizon +SP.movieName = movieName; + +SP.u0.min = SP.u0.min(:,ones(1,SP.hor_length+1)); +SP.u0.max = SP.u0.max(:,ones(1,SP.hor_length+1)); + +%% ------------------------------ PLOTTING ------------------------------------- + +SP.pred_actual = SP.pred; +SP = modify_predicates_rob_ball( SP ); +SP.pred_orig = SP.pred; + +% Plot the predicates +delete(hfig); + +hfig = figure; +hold on; +axis([-0.8 0.8 -0.6 0.6]); + + +Z = sdpvar(SP.n_outputs,1); +for ii = 1:SP.pred_known_sz + + if ii <= SP.safe_num_known + fill(SP.pred(ii).values(:,1), SP.pred(ii).values(:,2) , [0 0.5 0]); + end + + if SP.pred(ii).safe == -1 + colorval = [1 0 0]; + else + colorval = [0 1 0]; + end + plot(SP.pred(ii).A*Z <= SP.pred(ii).b, [], colorval); + + if ii > SP.safe_num_known + fill(SP.pred(ii).values(:,1), SP.pred(ii).values(:,2) , [0.5 0 0]); + end +end + +pause(0.01); + +%% -------------------------- RUN OPTIMIZATION --------------------------------- + +sw = tic; +sprintf('Encoding the controller ... ') +argvout_temp = generate_critical_constraints_MILP(SP); + +sprintf('Time taken to encode controller is : %d', toc(sw)) + +sprintf('Hit Enter to run the optimization.') +pause(); + +sprintf('Running the receding horizon controller now. Press "b" to put new unsafe sets in the environment.') +time_exec = tic; +argvout = MILP_find_OptimalTraj_mpc_m3pi(argvout_temp, plot_flag, hfig); + +sprintf('Total Time taken to solve: %d', toc(time_exec)) + +uvals = argvout.input.values; +yvals = argvout.SP.yout; + + + +%% ---------------------- SIMULATE THE FINAL RESULTING TRAJECTORY -------------- +SP = argvout.SP; +SP.u0.values = uvals; +SP.pred = SP.pred_actual; +SP.yout = yvals'; + + +[~, SP.tmin_con, SP.dmin_con] = staliro_distance(SP, SP.yout, SP.times'); + +figure(hfig); +plot(SP.yout(:,1), SP.yout(:,2), 'k-'); +plot(SP.yout(:,1), SP.yout(:,2), 'mo'); + + +sprintf('Minimum Robustness of %d is achieved at %d seconds', SP.dmin_con, ... + SP.tmin_con) +sprintf('Objective function value is %d', sum(sum(abs(SP.u0.values)))) + + +argvout.SP.u0.values = SP.u0.values; +argvout.SP.yout = SP.yout; +argvout.SP.dmin_con = SP.dmin_con; +argvout.SP.tmin_con = SP.tmin_con; + +end diff --git a/generate_critical_constraints_MILP.m b/generate_critical_constraints_MILP.m new file mode 100644 index 0000000..0091157 --- /dev/null +++ b/generate_critical_constraints_MILP.m @@ -0,0 +1,91 @@ +function argvout = generate_critical_constraints_MILP(SP) +% argvout = GENERATE_CRITICAL_CONSTRAINTS_MILP(SP) identifies all the critical +% constraints that need to be activated for the known environment in an +% open-loop fashion and encodes the MILP controller + +yalmip('clear') + +%% Discretize the system +SP.ss_cont= ss(SP.A, SP.B, SP.C, SP.D); +SP.tf_disc = c2d(SP.ss_cont, SP.ds); + +%% Setup the controller and the optimization process + +SP.Q = eye(SP.n); +SP.R = 1; + +SP.controller = setup_MILP_controller_all_cons(SP); + +x_cur = SP.x0; +SP.u0.values = zeros(SP.n_inputs, SP.hor_length + 1); + +%% Initialize other variables +SP.t0 = 0; +SP.tInd_con = 0; +SP.crit_pred = 0; +SP.phi = SP.phi_orig; + +SP.tmin_con = -1; +SP.dmin_con = -Inf; + +SP.constraints_tInd = zeros; +SP.crit_predVal = zeros; + +prev_tInd_ind = []; +sol_count = 1; + + +% set all constraints to be inactive ---> they are made active as required +SP.con_active = zeros(SP.pred_num, SP.hor_length+1); +SP.con_active_orig = SP.con_active; + +%% Start the main open-loop optimization procedure + +while((isempty(prev_tInd_ind) || (~isempty(prev_tInd_ind) && isempty(find... + (SP.crit_predVal(prev_tInd_ind) == SP.crit_pred, 1)))) && ... + (SP.dmin_con < 0 && SP.tmin_con ~= 0)) + + + SP.constraints_tInd(sol_count) = SP.tInd_con; + SP.crit_predVal(sol_count) = SP.crit_pred; + + + %=========================================================================== + % Compute Robustness and identify the critical constraint + %=========================================================================== + + [solutions,diagnostics] = SP.controller{{x_cur(:) , SP.con_active , ... + SP.hor_length+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.yout_disc = solutions{3}'; + + %=========================================================================== + % Compute Robustness and identify the critical constraint + %=========================================================================== + + + [~, SP.tmin_con, SP.dmin_con, ~, SP.crit_pred] = staliro_distance(SP, ... + SP.yout_disc, SP.times'); + + + % add constraints corresponding to the critical predicates + SP.tInd_con = round(SP.tmin_con/SP.ds)+1; + SP.con_active(SP.crit_pred, SP.tInd_con) = 1; + prev_tInd_ind = find(SP.constraints_tInd == SP.tInd_con); + + sol_count = sol_count + 1; + +end + +%% Output results + +argvout = SP; + +end + + diff --git a/get_system_description.m b/get_system_description.m new file mode 100644 index 0000000..063271d --- /dev/null +++ b/get_system_description.m @@ -0,0 +1,41 @@ +function SP = get_system_description() + + SP.u0.max = 0.25*[1 1]'; + SP.u0.min = -0.25*[1 1]'; + + SP.A = [0 0 1 0; + 0 0 0 1; + 0 0 0 0; + 0 0 0 0]; + + SP.Bu = [0 0 1 0;0 0 0 1]'; + SP.Bw = zeros(size(SP.Bu)); + SP.B = [SP.Bu SP.Bw]; + + SP.C = [1 0 0 0; + 0 1 0 0]; + + SP.Du = [0 0; 0 0]'; + SP.Dw = zeros(size(SP.Du)); + SP.D = [SP.Du SP.Dw]; + + SP.u0.A = SP.A; + SP.u0.B = SP.B; + % SP.u0.Bu = SP.Bu; + % SP.u0.Bw = SP.Bw; + SP.u0.C = SP.C; + SP.u0.D = SP.D; + % SP.u0.Du = SP.Du; + % SP.u0.Dw = SP.Dw; + + SP.u0.input_values_human = []; + + SP.n = size(SP.A,1); + SP.n_inputs = size(SP.Bu,2); + SP.n_outputs = size(SP.C,1); + + SP.x0 = zeros(SP.n,1); + + SP.observer_gains = 0.1*eye(3); + +end \ No newline at end of file diff --git a/ginputc.m b/ginputc.m new file mode 100644 index 0000000..5a61aef --- /dev/null +++ b/ginputc.m @@ -0,0 +1,424 @@ +function [x, y, button, ax] = ginputc(varargin) +%GINPUTC Graphical input from mouse. +% GINPUTC behaves similarly to GINPUT, except you can customize the +% cursor color, line width, and line style. +% +% [X,Y] = GINPUTC(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse. Data points are entered by pressing +% a mouse button or any key on the keyboard except carriage return, +% which terminates the input before N points are entered. +% Note: if there are multiple axes in the figure, use mouse clicks +% instead of key presses. Key presses may not select the axes +% where the cursor is. +% +% [X,Y] = GINPUTC gathers an unlimited number of points until the return +% key is pressed. +% +% [X,Y] = GINPUTC(N, PARAM, VALUE) and [X,Y] = GINPUTC(PARAM, VALUE) +% specifies additional parameters for customizing. Valid values for PARAM +% are: +% 'FigHandle' : Handle of the figure to activate. Default is gcf. +% 'Color' : A three-element RGB vector, or one of the MATLAB +% predefined names, specifying the line color. See +% the ColorSpec reference page for more information +% on specifying color. Default is 'k' (black). This is +% the cursor color. +% +% 'PointColor' : A three-element RGB vector, or one of the MATLAB +% predefined names, specifying the line color. See +% the ColorSpec reference page for more information +% on specifying color. Default is 'k' (black). This is +% the color of the points selected. As well as the color +% of the line joining the selected points. +% +% 'LineWidth' : A scalar number specifying the line width. +% Default is 0.5. +% 'LineStyle' : '-', '--', '-.', ':'. Default is '-'. +% 'ShowPoints' : TRUE or FALSE specifying whether to show the +% points being selected. Default is false. +% 'ConnectPoints' : TRUE or FALSE specifying whether to connect the +% points as they are being selected. This only +% applies when 'ShowPoints' is set to TRUE. Default +% is true. +% +% [X,Y,BUTTON] = GINPUTC(...) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was used +% (1,2,3 from left) or ASCII numbers if a key on the keyboard was used. +% +% [X,Y,BUTTON,AX] = GINPUTC(...) returns a fourth result, AX, that +% contains a vector of axes handles for the data points collected. +% +% Requires MATLAB R2007b or newer. +% +% Examples: +% [x, y] = ginputc; +% +% [x, y] = ginputc(5, 'Color', 'r', 'LineWidth', 3); +% +% [x, y, button] = ginputc(1, 'LineStyle', ':'); +% +% subplot(1, 2, 1); subplot(1, 2, 2); +% [x, y, button, ax] = ginputc; +% +% [x, y] = ginputc('ShowPoints', true, 'ConnectPoints', true); +% +% See also GINPUT, GTEXT, WAITFORBUTTONPRESS. + +% Jiro Doke +% October 19, 2012 +% Copyright 2012 The MathWorks, Inc. + +try + if verLessThan('matlab', '7.5') + error('ginputc:Init:IncompatibleMATLAB', ... + 'GINPUTC requires MATLAB R2007b or newer'); + end +catch %#ok + error('ginputc:Init:IncompatibleMATLAB', ... + 'GINPUTC requires MATLAB R2007b or newer'); +end + + +% Check input arguments +p = inputParser(); + +addOptional(p, 'N', inf, @(x) validateattributes(x, {'numeric'}, ... + {'scalar', 'integer', 'positive'})); +addParamValue(p, 'FigHandle', [], @(x) numel(x)==1 && ishandle(x)); +addParamValue(p, 'Color', 'k', @colorValidFcn); +addParamValue(p, 'PointColor', 'k', @colorValidFcn); +addParamValue(p, 'LineWidth', 0.5 , @(x) validateattributes(x, ... + {'numeric'}, {'scalar', 'positive'})); +addParamValue(p, 'LineStyle', '-' , @(x) validatestring(x, ... + {'-', '--', '-.', ':'})); +addParamValue(p, 'ShowPoints', false, @(x) validateattributes(x, ... + {'logical'}, {'scalar'})); +addParamValue(p, 'ConnectPoints', true, @(x) validateattributes(x, ... + {'logical'}, {'scalar'})); + +parse(p, varargin{:}); + +N = p.Results.N; +hFig = p.Results.FigHandle; +color = p.Results.Color; +pointcolor = p.Results.PointColor; +linewidth = p.Results.LineWidth; +linestyle = p.Results.LineStyle; +showpoints = p.Results.ShowPoints; +connectpoints = p.Results.ConnectPoints; + + +%-------------------------------------------------------------------------- + function tf = colorValidFcn(in) + % This function validates the color input parameter + + validateattributes(in, {'char', 'double'}, {'nonempty'}); + if ischar(in) + validatestring(in, {'b', 'g', 'r', 'c', 'm', 'y', 'k', 'w'}); + else + assert(isequal(size(in), [1 3]) && all(in>=0 & in<=1), ... + 'ginputc:InvalidColorValues', ... + 'RGB values for "Color" must be a 1x3 vector between 0 and 1'); + % validateattributes(in, {'numeric'}, {'size', [1 3], '>=', 0, '<=', 1}) + end + tf = true; + end +%-------------------------------------------------------------------------- + +if isempty(hFig) + hFig = gcf; +else + figure(hFig); +end + +% Try to get the current axes even if it has a hidden handle. +hAx = get(hFig, 'CurrentAxes'); +if isempty(hAx) + allAx = findall(hFig, 'Type', 'axes'); + if ~isempty(allAx) + hAx = allAx(1); + else + hAx = axes('Parent', hFig); + end +end + +% Handle interactive properites of HG objects. Save the current settings so +% that they can be restored later +allHG = findall(hFig); +propsToChange = {... + 'WindowButtonUpFcn', ... + 'WindowButtonDownFcn', ... + 'WindowButtonMotionFcn', ... + 'WindowKeyPressFcn', ... + 'WindowKeyReleaseFcn', ... + 'ButtonDownFcn', ... + 'KeyPressFcn', ... + 'KeyReleaseFcn', ... + 'ResizeFcn'}; +validObjects = false(length(allHG), length(propsToChange)); +curCallbacks = cell(1, length(propsToChange)); + +% Save current properties and set them to '' +for id = 1:length(propsToChange) + validObjects(:, id) = isprop(allHG, propsToChange{id}); + curCallbacks{id} = get(allHG(validObjects(:, id)), propsToChange(id)); + set(allHG(validObjects(:, id)), propsToChange{id}, ''); +end + +% Save current pointer +curPointer = get(hFig, 'Pointer'); +curPointerShapeCData = get(hFig, 'PointerShapeCData'); + +% Change window functions +set(hFig, ... + 'WindowButtonDownFcn', @mouseClickFcn, ... + 'WindowButtonMotionFcn', @mouseMoveFcn, ... + 'KeyPressFcn', @keyPressFcn, ... + 'ResizeFcn', @resizeFcn, ... + 'Pointer', 'custom', ... + 'PointerShapeCData', nan(16, 16)); + +% Create an invisible axes for displaying the full crosshair cursor +hInvisibleAxes = axes(... + 'Parent', hFig, ... + 'Units', 'normalized', ... + 'Position', [0 0 1 1], ... + 'XLim', [0 1], ... + 'YLim', [0 1], ... + 'HitTest', 'off', ... + 'HandleVisibility', 'off', ... + 'Visible', 'off'); + +% Create line object for the selected points +if showpoints + if connectpoints + pointsLineStyle = '-'; + else + pointsLineStyle = 'none'; + end + + selectedPoints = []; + hPoints = line(nan, nan, ... + 'Parent', hInvisibleAxes, ... + 'HandleVisibility', 'off', ... + 'HitTest', 'off', ... + 'Color', pointcolor, ... + 'Marker', 'o', ... + 'MarkerFaceColor', [1 .7 .7], ... + 'MarkerEdgeColor', pointcolor, ... + 'LineStyle', pointsLineStyle); +end + +% Create tooltip for displaying selected points +hTooltipControl = text(0, 1, 'HIDE', ... + 'Parent', hInvisibleAxes, ... + 'HandleVisibility', 'callback', ... + 'FontName', 'FixedWidth', ... + 'VerticalAlignment', 'top', ... + 'HorizontalAlignment', 'left', ... + 'BackgroundColor', [.5 1 .5]); +hTooltip = text(0, 0, 'No points', ... + 'Parent', hInvisibleAxes, ... + 'HandleVisibility', 'off', ... + 'HitTest', 'off', ... + 'FontName', 'FixedWidth', ... + 'VerticalAlignment', 'top', ... + 'HorizontalAlignment', 'left', ... + 'BackgroundColor', [1 1 .5]); + +% Call resizeFcn to update tooltip location +resizeFcn(); + +% Create full crosshair lines +hCursor = line(nan, nan, ... + 'Parent', hInvisibleAxes, ... + 'Color', color, ... + 'LineWidth', linewidth, ... + 'LineStyle', linestyle, ... + 'HandleVisibility', 'off', ... + 'HitTest', 'off'); + +% Prepare results +x = []; +y = []; +button = []; +ax = []; + +% Wait until enter is pressed. +uiwait(hFig); + + +%-------------------------------------------------------------------------- + function mouseMoveFcn(varargin) + % This function updates cursor location based on pointer location + + cursorPt = get(hInvisibleAxes, 'CurrentPoint'); + + set(hCursor, ... + 'XData', [0 1 nan cursorPt(1) cursorPt(1)], ... + 'YData', [cursorPt(3) cursorPt(3) nan 0 1]); + end +%-------------------------------------------------------------------------- + +%-------------------------------------------------------------------------- + function mouseClickFcn(varargin) + % This function captures mouse clicks. + % If the tooltip control is clicked, then toggle tooltip display. + % If anywhere else is clicked, record point. + + if isequal(gco, hTooltipControl) + tooltipClickFcn(); + else + updatePoints(get(hFig, 'SelectionType')); + end + end +%-------------------------------------------------------------------------- + +%-------------------------------------------------------------------------- + function keyPressFcn(obj, edata) %#ok + % This function captures key presses. + % If "return", then exit. + % If "delete" (or "backspace"), then delete previous point. + % If any other key, record point. + + key = double(edata.Character); + if isempty(key) + return; + end + + switch key + case 13 % return + exitFcn(); + + case {8, 127} % delete or backspace + if ~isempty(x) + x(end) = []; + y(end) = []; + button(end) = []; + ax(end) = []; + + if showpoints + selectedPoints(end, :) = []; + set(hPoints, ... + 'XData', selectedPoints(:, 1), ... + 'YData', selectedPoints(:, 2)); + end + + displayCoordinates(); + end + + otherwise + updatePoints(key); + + end + end +%-------------------------------------------------------------------------- + +%-------------------------------------------------------------------------- + function updatePoints(clickType) + % This function captures the information for the selected point + + hAx = gca; + pt = get(hAx, 'CurrentPoint'); + x = [x; pt(1)]; + y = [y; pt(3)]; + ax = [ax; hAx]; + + if ischar(clickType) % Mouse click + switch lower(clickType) + case 'open' + clickType = 1; + case 'normal' + clickType = 1; + case 'extend' + clickType = 2; + case 'alt' + clickType = 3; + end + end + button = [button; clickType]; + + displayCoordinates(); + + if showpoints + cursorPt = get(hInvisibleAxes, 'CurrentPoint'); + selectedPoints = [selectedPoints; cursorPt([1 3])]; + set(hPoints, ... + 'XData', selectedPoints(:, 1), ... + 'YData', selectedPoints(:, 2)); + end + + % If captured all points, exit + if length(x) == N + exitFcn(); + end + end +%-------------------------------------------------------------------------- + +%-------------------------------------------------------------------------- + function tooltipClickFcn() + % This function toggles the display of the tooltip + + if strcmp(get(hTooltipControl, 'String'), 'SHOW') + set(hTooltipControl, 'String', 'HIDE'); + set(hTooltip, 'Visible', 'on'); + else + set(hTooltipControl, 'String', 'SHOW'); + set(hTooltip, 'Visible', 'off'); + end + end +%-------------------------------------------------------------------------- + +%-------------------------------------------------------------------------- + function displayCoordinates() + % This function updates the coordinates display in the tooltip + + if isempty(x) + str = 'No points'; + else + str = sprintf('%d: %0.3f, %0.3f\n', [1:length(x); x'; y']); + str(end) = ''; + end + set(hTooltip, ... + 'String', str); + end +%-------------------------------------------------------------------------- + +%-------------------------------------------------------------------------- + function resizeFcn(varargin) + % This function adjusts the position of tooltip when the figure is + % resized + + sz = get(hTooltipControl, 'Extent'); + set(hTooltip, 'Position', [0 sz(2)]); + end +%-------------------------------------------------------------------------- + +%-------------------------------------------------------------------------- + function exitFcn() + % This function exits GINPUTC and restores previous figure settings + + for idx = 1:length(propsToChange) + set(allHG(validObjects(:, idx)), propsToChange(idx), curCallbacks{idx}); + end + + % Restore window functions and pointer + % set(hFig, 'WindowButtonDownFcn', curWBDF); + % set(hFig, 'WindowButtonMotionFcn', curWBMF); + % set(hFig, 'WindowButtonUpFcn', curWBUF); + % set(hFig, 'KeyPressFcn', curKPF); + % set(hFig, 'KeyReleaseFcn', curKRF); + % set(hFig, 'ResizeFcn', curRF); + + % Restore pointer + set(hFig, 'Pointer', curPointer); + set(hFig, 'PointerShapeCData', curPointerShapeCData); + + % Delete invisible axes and return control + delete(hInvisibleAxes); + uiresume(hFig); + end +%-------------------------------------------------------------------------- + +end \ No newline at end of file diff --git a/kth2R.m b/kth2R.m new file mode 100644 index 0000000..c6a91fd --- /dev/null +++ b/kth2R.m @@ -0,0 +1,31 @@ +function R = kth2R(k,th) +% kth2r outputs a rotation matrix R corresponding to the rotation of +% theta angle in radian by the axis represented bu the 3x1 vector k +% +% R = kth2R( k, th ) +% IN: +% k - vector around which the rotation will be performed +% th - rotation angle +% +% +% OUT: +% R - rotation matrix +% +% BY: Sayan Saha + +knorm = norm(k,2); +% if( numel( k ) ~= 3 ) +% fprintf( 'rotationVector should have 3 coordinates!\r' ); +% return; +% end + +k = reshape( k, 3, 1 ); +% if( knorm > 0 ) + k = k / knorm; + kcross = getCross(k); + R = eye(3) + sin(th)*kcross + (1-cos(th))*(kcross)^2; +% else +% fprintf( 'rotation Vector cannot be 0\n\r' ); +% end + +end \ No newline at end of file diff --git a/modd.m b/modd.m new file mode 100644 index 0000000..c10569f --- /dev/null +++ b/modd.m @@ -0,0 +1,8 @@ +function y = modd(x,n) +% MODD - Element-wise modulo operator +% This function returns a vector x modulo n, and if any of the +% elements are zero, they are replaced with n + +y = mod(x,n); +y(y == 0) = n; + diff --git a/modify_predicates_rob_ball.m b/modify_predicates_rob_ball.m new file mode 100644 index 0000000..ff7e5c4 --- /dev/null +++ b/modify_predicates_rob_ball.m @@ -0,0 +1,17 @@ +function SP = modify_predicates_rob_ball( SP ) + + +sz_pred = size(SP.pred,2); + +% if sz_pred ~= SP.pred_known_sz +% indxs = SP.pred_known_sz+1:sz_pred; +% else + indxs = 1:sz_pred; +% end + +for ii = indxs + SP.pred(ii).b = bsxfun(@minus, SP.pred(ii).b, SP.rob_ball_des*SP.pred(ii).safe); +end + +end + diff --git a/normc.m b/normc.m new file mode 100644 index 0000000..3198acd --- /dev/null +++ b/normc.m @@ -0,0 +1,5 @@ +function [ B ] = normc( A ) +%NORMC normalize the columns of a matrix to unit vectors +B = A ./ (ones(size(A,1),1)*sqrt(sum(A.^2))); +end + diff --git a/order_pts_center.m b/order_pts_center.m new file mode 100644 index 0000000..9d62ad4 --- /dev/null +++ b/order_pts_center.m @@ -0,0 +1,10 @@ +function pts = order_pts_center( pts ) + +center = mean(pts); + +% Order points by angle around center starting from 0 to 2 pi +angles = mod( atan2(pts(:,2)-center(2), pts(:,1)-center(1)), 2*pi ); +pts_ordered = sortrows([angles pts], 1); +pts = pts_ordered(:,2:end); + +end \ No newline at end of file diff --git a/pts2cvxpoly.m b/pts2cvxpoly.m new file mode 100644 index 0000000..32db5bd --- /dev/null +++ b/pts2cvxpoly.m @@ -0,0 +1,38 @@ +function [ A, b, pts ] = pts2cvxpoly( pts ) +%PTS2CVXPOLY Converts a set of points to a convex polygon representation +% A convex polygon can be represented by a set of vectors stored in a +% matrix "A" (vectors transposed and stacked) each one normal to one of +% the edges, and a column vectors "b" storing the distance from "center" +% to the edge specified by the corresponding normal at the same row in +% "A." + +center = mean(pts); + +% Order points by angle around center starting from 0 to 2 pi +angles = mod( atan2(pts(:,2)-center(2), pts(:,1)-center(1)), 2*pi ); +pts_ordered = sortrows([angles pts], 1); +pts = pts_ordered(:,2:end); + +% Check Convexity +n_pts = size(pts,1); +rp = pts(1:n_pts,:); % Right points (looking out from inside) +mp = pts([2:n_pts, 1], :); % Middle points " " +lp = pts([3:n_pts, 1, 2], :); % Left points " " +% For each set of three consecutive points, take the dot product between +% the vector from right point to left point and the vector from right +% point to middle point. Will be positive if convex +convex_pts = (mp(:,1) - rp(:,1)).*(lp(:,2) - rp(:,2)) + ... + (mp(:,2) - rp(:,2)).*(rp(:,1) - lp(:,1)); +if ~all(convex_pts >= 0) + error('Points must define a convex polygon.') +end + +% Matrix of norms, which are perpendicular to edges +A = [mp(:,2) - rp(:,2), rp(:,1) - mp(:,1)]; +A = normc(A')'; + +% distance = pt dot product norm of edge +b = diag(A*pts'); + +end + diff --git a/rot.m b/rot.m new file mode 100644 index 0000000..e359610 --- /dev/null +++ b/rot.m @@ -0,0 +1,5 @@ +function R=rot(k,theta) + + k=k/norm(k); + R=eye(3,3)+sin(theta)*hat(k)+(1-cos(theta))*hat(k)*hat(k); + \ No newline at end of file diff --git a/select_Predicates.m b/select_Predicates.m new file mode 100644 index 0000000..5ff54d6 --- /dev/null +++ b/select_Predicates.m @@ -0,0 +1,211 @@ +function SP = select_Predicates(SP, userInput) +% SELECT_PREDICATES returns predicates in polyhedral form Input is SP, userInput +% Output is SP with the field SP.pred + +% this SP.pred are in the form of (AX <= b) and are required in computing +% the signedDistance, called in staliro_distance file. + +% predicates are defined in terms of (x,y) position of the mobile robot in the +% 2D plane + + +% Example set representation represented by +% 1 <= y <= 10, 7 <= x <= 9. +% y <= 10 ---> A(1,2) = 1, b(1) = 1 +% 1 <= y ---> -y <= -1 ---> A(3,2) = -1, b(3) = -1 +% 7 <= x ---> -x <= -7 ---> A(2,1) = -1, b(2) = -7 +% x <= 9 ---> A(4,1) = 1, b(4) = 9 +% the above process can be automated with the the PTS2CVXPOLY function + +% SP.safe ---> indicates if a negation is before the predicate ---> used in +% modify_SP.pred_rob_ball.m + + +str_name = ('a':'z').'; + + +if ~strcmp(userInput, 'yes') + + + + % create new figure + hfig = figure; + hold on; + axis([-0.8 0.8 -0.6 0.6]); + + pos_init = [650 -300]/1000; + + SP.pred_known_sz = 2; % We know Goal and one Obstacle from the begining + % of the path planning process + SP.pred_unknown_sz = 1; % We assume we might encounter one more + % Obstacle in future + + + SP.pred_hor = 40; + SP.tf = SP.pred_hor*SP.ds; + + % Goal Set + SP.pred(1).values = [-294 544; -292 189; -557 201; -563 550]/1000; + SP.pred(1).safe = 1; + + % Known Obstacle + SP.pred(2).values = [299 -46; 309 -394; -6 -391; 3 -50]/1000; + SP.pred(2).safe = -1; + + % UnKnown Obstacle + SP.pred(3).values = bsxfun(@plus, 0.5*SP.pred(2).values, [-50 250]/1000); + SP.pred(3).safe = -1; + + SP.phi_not_global = strcat('[]_[',num2str((SP.pred_hor-5)*SP.ds), ... + ',', num2str(SP.tf),']a'); + SP.phi_global = '[]!b'; + SP.phi_global_unknown(1).str = '[]!c'; + + SP.safe_num_known = 1; + SP.unsafe_num_known = 1; + +else + + + + %% Get information about number and type of predicates + + SP.safe_num_known = input('\n\n How many known safe sets do you want to choose? '); + timeStamps = input('\n\n Enter the time-points during which you want to be in the \n safe sets in ascending manner such that the first value \n is when you enter the set and the second value is when you leave the set. '); + timeStamps = timeStamps(:)*SP.ds; + temp = sort(timeStamps, 'ascend'); + + if size(timeStamps,1) ~= SP.safe_num_known*2 + error('Size of the timeStamps variable should be twice that of the number of safe sets you want to visit.'); + end + + if temp ~= timeStamps + error('Enter the time-points in ascending manner.'); + end + + SP.unsafe_num_known = input('\n\n How many known unsafe sets do you want to choose? '); + SP.pred_unknown_sz = input('\n\n How many unknown unsafe sets do you want to choose? '); + + + % create new figure + hfig = figure; + hold on; + axis([-0.8 0.8 -0.6 0.6]); + + %% Select Initial Position + + + h_title = title('Choose the initial position of the mobile robot.'); + % sprintf('Choose the initial position of the mobile robot. ') + pos_init = ginput(1); + + SP.phi_not_global = ''; + + + %% Goal Sets + for jj = 1:SP.safe_num_known + + if jj==1 + h_title.String = 'Select the First Safe Set.'; + % sprintf('Select the First Safe Set.') + else + h_title.String = 'Select the Next Safe Set.'; + % sprintf('Select the Next Safe Set.') + end + + [xpos, ypos] = ginputc('FigHandle', hfig, 'PointColor', [0 0.5 0], ... + 'Color', [0 0.5 0], 'LineWidth', 2, 'ShowPoints', true, ... + 'ConnectPoints', true); + + SP.pred(jj).values = [xpos ypos]; + + SP.pred(jj).safe = 1; + fill(SP.pred(jj).values(:,1), SP.pred(jj).values(:,2) , [0 0.5 0]); + + SP.phi_not_global = strcat(SP.phi_not_global, '[]_[',num2str... + (timeStamps(2*jj-1)), ',', num2str(timeStamps(2*jj)),']', str_name(jj)); + + if jj ~= SP.safe_num_known + SP.phi_not_global = strcat(SP.phi_not_global, ' /\ '); + end + SP.pred(jj).safe = 1; + end + + SP.tf = timeStamps(end); + SP.pred_hor = SP.tf/SP.ds; + SP.phi_global = ''; + + + %% Unsafe Sets + + + for jj = (SP.safe_num_known+1):(SP.safe_num_known + SP.unsafe_num_known) + + if jj==SP.safe_num_known+1 + h_title.String = 'Select the first known Unsafe Set.'; + % sprintf('Select the first known Unsafe Set.') + else + h_title.String = 'Select the next known Unsafe Set.'; + % sprintf('Select the next known unsafe Set.') + end + + [xpos, ypos] = ginputc('FigHandle', hfig, 'PointColor', [0.5 0 0], ... + 'Color', [0.5 0 0], 'LineWidth', 2, 'ShowPoints', true, ... + 'ConnectPoints', true); + SP.pred(jj).values = [xpos ypos]; + SP.pred(jj).safe = -1; + fill(SP.pred(jj).values(:,1), SP.pred(jj).values(:,2) , [0.5 0 0]); + + SP.phi_global = strcat(SP.phi_global, '[]!', str_name(jj)); + if jj ~= (SP.safe_num_known + SP.unsafe_num_known) + SP.phi_global = strcat(SP.phi_global, ' /\ '); + end + SP.pred(jj).safe = -1; + + end + + SP.pred_known_sz = jj; + + + %% Future Unsafe Sets + + + for jj = (SP.pred_known_sz+1):(SP.pred_known_sz+SP.pred_unknown_sz) + + + if jj==SP.pred_known_sz+1 + h_title.String = 'Select the first unknown Unsafe Set.'; + % sprintf('Select the first unknown Unsafe Set.') + else + h_title.String = 'Select the next unknown Unsafe Set.'; + % sprintf('Select the next unknown unsafe Set.') + end + + [xpos, ypos] = ginputc('FigHandle', hfig, 'PointColor', [0.75 0 0], ... + 'Color', [0.75 0 0], 'LineWidth', 2, 'ShowPoints', true, ... + 'ConnectPoints', true); + + SP.pred(jj).values = [xpos ypos]; + SP.pred(jj).safe = -1; + fill(SP.pred(jj).values(:,1), SP.pred(jj).values(:,2) , [0.75 0 0]); + + SP.phi_global_unknown(jj-SP.pred_known_sz).str = strcat('[]!', ... + str_name(jj)); + + end + +end + +for ii = 1:(SP.pred_known_sz+SP.pred_unknown_sz) + + SP.pred(ii).str = str_name(ii); + [SP.pred(ii).A, SP.pred(ii).b] = pts2cvxpoly(SP.pred(ii).values); + +end + +SP.phi_orig = [SP.phi_not_global '/\' SP.phi_global]; +SP.x0(1:2) = pos_init; % SP.x0 = [x-pos y-pos x-dot y-dot]' +SP.hfig = hfig; + + +end \ No newline at end of file diff --git a/select_vertices.m b/select_vertices.m new file mode 100644 index 0000000..8e947a9 --- /dev/null +++ b/select_vertices.m @@ -0,0 +1,49 @@ +function select_vertices(hfig) + +global pos_click; + +set(hfig,'WindowButtonDownFcn',@wbdcb) +ah = axes('SortMethod','childorder'); +axis([-0.8 0.8 -0.6 0.6]); + + + function wbdcb(src,callbackdata) + if strcmp(src.SelectionType,'normal') + src.Pointer = 'circle'; + cp = ah.CurrentPoint; + + xinit = cp(1,1); + yinit = cp(1,2); + + pos_click(size(pos_click,1)+1, 1:2) = cp(1,1:2); + + hl = line('XData',xinit,'YData', yinit, 'Marker','p','color','b'); + src.WindowButtonMotionFcn = @wbmcb; + src.WindowButtonUpFcn = @wbucb; + end + + function wbmcb(src,callbackdata) + + cp = ah.CurrentPoint; + xdat = [xinit,cp(1,1)]; + ydat = [yinit,cp(1,2)]; + + hl.XData = xdat; + hl.YData = ydat; + + drawnow + end + + function wbucb(src,callbackdata) + + if strcmp(src.SelectionType,'alt') + src.Pointer = 'arrow'; + src.WindowButtonMotionFcn = ''; + src.WindowButtonUpFcn = ''; + else + return + end + + end + end +end \ No newline at end of file diff --git a/setup_MILP_controller_all_cons.m b/setup_MILP_controller_all_cons.m new file mode 100644 index 0000000..b9bef6d --- /dev/null +++ b/setup_MILP_controller_all_cons.m @@ -0,0 +1,110 @@ +function controller = setup_MILP_controller_all_cons(SP) + % controller = setup_MILP_controller_all_cons(SP) encodes the MILP controller + + %% define variables to be optimized + u = sdpvar(repmat(SP.n_inputs,1, SP.hor_length + 1),ones(1,SP.hor_length + 1)); % control signal variable + x = sdpvar(repmat(SP.n,1, SP.hor_length+2),ones(1,SP.hor_length+2)); % state space variable + y = sdpvar(repmat(SP.n_outputs,1, SP.hor_length + 1),ones(1,SP.hor_length + 1)); % output variable + w = sdpvar(repmat(SP.n_inputs,1, SP.hor_length + 1),ones(1,SP.hor_length + 1)); % disturbance signal variable + + con_active = sdpvar(SP.pred_num, SP.hor_length+1); % tracks which constraints to be set to active + d = {}; % bigM variable required for placing constraints to avoid unsafe sets ---> see below in the constraints section + M = 1000; % bigM value + x_cur = sdpvar(SP.n,1); + rem_hor_length = sdpvar(1,1); +% hor_length = sdpvar(1,1); + + % account for unknown predicates ----------------------------------- + % ------------------------------------------------------------------ + for ii = 1:SP.pred_num % SP.pred_known_sz + pred_A(ii).vals = sdpvar(size(SP.pred(ii).A,1), size(SP.pred(ii).A,2)); + pred_b(ii).vals = sdpvar(size(SP.pred(ii).b,1), size(SP.pred(ii).b,2)); + end + + %% initialize constraints and objective + + uVec = [u{:}]; + objective = sum(sum(abs(uVec))); % norm(uVec,1); % + + constraints = []; + + if isempty(SP.u0.input_values_human) + SP.u0.input_values_human = zeros(size(u)); + end + + constraints = [constraints, x{1} == x_cur]; + + for k = 1:SP.hor_length+1 + % add system dynamics as constraints + constraints = [constraints, x{k+1} == SP.tf_disc.A*x{k}+SP.tf_disc.B*[u{k}; w{k}]]; + constraints = [constraints, y{k} == SP.tf_disc.C*x{k}+SP.tf_disc.D*[u{k}; w{k}]]; + + % add bounds on control input signal as constraints + constraints = [constraints, SP.u0.min(:,k) <= u{k} <= SP.u0.max(:,k)]; + + if k > value(rem_hor_length) + constraints = [constraints, u{k} == zeros(SP.n_inputs,1)]; + end + +% if k == 1 && value(rem_hor_length) == (SP.hor_length+1) +% constraints = [constraints, u{k} ~= zeros(SP.n_inputs,1)]; +% end + + end + + %% encode all the constraints + sz_d = 0; + + for ii = 1:SP.pred_num + + if SP.pred(ii).safe == -1 % predicates to be avoided + + for jj = 2:SP.hor_length+1 + + sz_d = sz_d + 1; + + if ii > SP.pred_known_sz + sz_pred = 4; + else + sz_pred = size(SP.pred(ii).b,1); % number of hyperplanes constructing the predicate + end + + d{sz_d} = binvar(sz_pred,1); % update the variable holding the bigM variables + + % add constraint such that the state is outside of atleast one of the hyperplanes + constraints = [constraints, sum(d{sz_d}(:,1))*con_active(ii,jj) <= (sz_pred-1)*con_active(ii,jj)]; + + for kk = 1:sz_pred + constraints = [constraints, (pred_A(ii).vals(kk,:)*y{jj} ... + - pred_b(ii).vals(kk))*con_active(ii,jj) >= -M*d{sz_d}(kk,1)*con_active(ii,jj)]; + end + end + + else % predicates to be visited + + for jj = 2:SP.hor_length+1 + constraints = [constraints, pred_A(ii).vals*y{jj}*con_active(ii,jj) <= pred_b(ii).vals*con_active(ii,jj)]; + end + + end + end + + + % define what the controller inputs are + parameters_in = {x_cur, con_active, rem_hor_length, [w{:}], pred_A.vals, pred_b.vals}; + + % define what the controller outputs are + solutions_out = {[u{:}], [x{:}], [y{:}]}; + + + %% define settings + + % verbose = 2 ---> gives detailed iteration wise results ---> good for debugging + % change solvername to use a different solver than gurobi + % if no solver option is provided yalmip uses the default in-built one + + ops = sdpsettings('verbose', 1, 'solver', 'gurobi' ); + controller = optimizer(constraints, objective, ops,parameters_in,solutions_out); + + +end \ No newline at end of file diff --git a/staliro_distance.m b/staliro_distance.m new file mode 100644 index 0000000..82d81ad --- /dev/null +++ b/staliro_distance.m @@ -0,0 +1,13 @@ +function [nearest_point_on_s, tmin, dmin, umin, i_pr, inSet] = staliro_distance(SP,xout,tout) +[rob, aux] = dp_taliro(SP.phi,SP.pred,xout,tout); +dmin = rob; +if aux.i==0 + aux.i=1; +end +tmin = tout(aux.i); +nearest_point_on_s = xout(aux.i,:)'; +i_pr = aux.pred;%get_predicate_index(aux.predicate,SP.pred); +% disp(['i_pr = ',num2str(i_pr)]) +[~,inSet,umin] = SignedDist(nearest_point_on_s,SP.pred(i_pr).A,SP.pred(i_pr).b); +% umin is the projection of the critical point of the trajectory on the critical predicate of the MTL specification +end \ No newline at end of file diff --git a/trans.m b/trans.m new file mode 100644 index 0000000..817b5dd --- /dev/null +++ b/trans.m @@ -0,0 +1,5 @@ +function new_coords=trans(coords,R,r) +% TRANS - transforms the given coordinates by rotating them by R +% then translating the coordinates by r + new_coords = R*coords + ... + r*ones(1,size(coords, 2)); \ No newline at end of file diff --git a/update_car_paths.m b/update_car_paths.m new file mode 100644 index 0000000..b614669 --- /dev/null +++ b/update_car_paths.m @@ -0,0 +1,39 @@ +function [objsC, time_disp] = update_car_paths(objsC, time_disp, count, Ts, lt) + +cObjsC = size(objsC,2); + +for i = 1:cObjsC + objsC(i).disp_st(objsC(i).ptr,:)=objsC(i).z(1:3)'; +end + +% The trajectory data is stored in disp_st, but this is of +% fixed length, and the data keeps overwriting the data in a +% loop. "ptr" stores the index of the oldest recorded point, +% and essentially creates an ordered matrix by +% xd = [ disp_st(old_pt:end,1) disp_st(1:old_pt-1) ] +% and similarly for yd. + +for i = 1:cObjsC + xd = objsC(i).disp_st(modd(objsC(i).ptr-lt+1:objsC(i).ptr,lt),1); + yd = objsC(i).disp_st(modd(objsC(i).ptr-lt+1:objsC(i).ptr,lt),2); + zd = objsC(i).disp_st(modd(objsC(i).ptr-lt+1:objsC(i).ptr,lt),3); + objsC(i).ptr = objsC(i).ptr+1; + objsC(i).ptr = modd(objsC(i).ptr,lt); + + % Erase these plots on before replotting +% set(objsC(i).handle, 'erasemode', 'normal'); + + + % Update the trajectory data + set(objsC(i).handle,'XData', xd, 'YData', yd, 'ZData', zd); + +end + +% Update the time data +time_now = strcat('TIME = ',num2str(count*Ts), 'sec'); +set(time_disp,'String',time_now); + +update_plots(objsC); +% drawnow; + +end diff --git a/update_plots.m b/update_plots.m new file mode 100644 index 0000000..02f48de --- /dev/null +++ b/update_plots.m @@ -0,0 +1,26 @@ +function update_plots(objsC) +% UPDATE_PLOTS(objsC) - updates based on the current state z +% objsC - car objects + + +cObjsC = size(objsC,2); + + +for j = 1:cObjsC + + pos_car = objsC(j).z(1:3,:); + R_car = rot([0 0 1], objsC(j).th); + + for i = 1:4 + coords = trans(objsC(j).arm(i).wheels.coords, R_car, pos_car); + set(objsC(j).arm(i).wheels.plot, 'XData', coords(1,:), ... + 'YData', coords(2,:), ... + 'ZData', coords(3,:)); + + end + + objsC(j).vertices = pos_car(:,ones(1,length(objsC(j).vertices))) + R_car*objsC(j).verts_init; + + set(objsC(j).Handle,'Vertices',objsC(j).vertices'); + +end \ No newline at end of file diff --git a/wrapToPi.m b/wrapToPi.m new file mode 100644 index 0000000..c39124b --- /dev/null +++ b/wrapToPi.m @@ -0,0 +1,44 @@ +function a = wrapToPi(a,a_center) +% function a = wrap(a,a_center) +% +% Wraps angles to a range of 2*pi. +% Inverse of Matlab's "unwrap", and better than wrapToPi ( which has +% redundant [-pi,pi]) +% Optional input "a_center" defines the center angle. Default is 0, giving +% angles from (-pi,pi], chosen to match angle(complex(-1,0)). Maximum +% possible value is pi. + +% T.Hilmer, UH +% 2010.10.18 version 2 +% removed code from version 1. Have not bug-checked second input +% "a_center" + +if nargin < 2, a_center = 0; end + +% new way +a = mod(a,2*pi); % [0 2pi) + +% shift +j = a > pi - a_center; +a(j) = a(j) - 2*pi; +j = a < a_center - pi; +a(j) = a(j) + 2*pi; + + +function a = old(a,arg) +% old way of doing it + +% wraps down to 2*[-pi,pi]: +a = rem(a,2*pi);% factors of 2pi are redundant + + +% default shift to [0,2pi): +j = a < 0; % negative angles +a(j) = a(j) + 2*pi; % wrapped to positive +a(a == 2*pi) = 0;% redundant + +if nargin > 1 && arg == 2*pi; return, end + +% shifts to (-pi,pi]: +j = a > pi; +a(j) = a(j) - 2*pi;