Minor revisions.

MILP_find_OptimalTraj_mpc_m3pi.m

@@ -4,22 +4,21 @@
 
 %% Initialize variables
 
-tIndxs = 1:SP.pred_hor+1;
-x_cur = SP.x0;
+tIndxs = 1:SP.pred_hor+1; % time indices corresponding to horizon length
+x_cur = SP.x0; % current state of the system
+
 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);
+uvals = zeros(SP.n_inputs, SP.hor_length + 1); % dummy variable to store control signals to the system
+yvals = zeros(SP.n_outputs, SP.hor_length + 1); % dummy variable to store output of the system
 solutions{3} = zeros(SP.n_outputs, SP.input_length);
-SP.yout = zeros(SP.n_outputs, SP.input_length);
 
-SP.sol_count_vals = zeros;
+SP.yout = zeros(SP.n_outputs, SP.input_length); % stores the actual trajectory of the system
 
-SP.states_system_prev = SP.states_system;
-SP.states_system_estimate = SP.states_system;
+SP.sol_count_vals = zeros;
 
 unknown_pred_count = 0;
 
-%% Create unicycle agents for plotting and other plot variables initialization
+%% Create unicycle robot for plotting and other plot variables initialization
 
 if plot_flag
 
@@ -49,16 +48,14 @@
 
   iter_time = tic;
 
-  sol_count = 1;
+  sol_count = 1;  % keeps track of how many iterations were required to solve the problem
   SP.tIndxs = tIndxs;
 
-  SP.states_system_prev = SP.states_system;
-
   SP.yout(:,ii) = x_cur(1:SP.n_outputs);
 
 
   %=============================================================================
-  % get new predicate online
+  % get new predicate online by pressing the key 'b'
   %=============================================================================
   if (strcmpi(get(hfig,'currentch'),'b')) && unknown_pred_count < SP.pred_unknown_sz
 
@@ -77,53 +74,58 @@
     SP.phi = strcat(SP.phi, ' /\ ', SP.phi_global_unknown(unknown_pred_count).str);    
   end
 
-  SP.dmin_con = -Inf;
+  SP.dmin_con = -Inf; % initialize robustness radius to -Infinity 
 
   %=============================================================================
   % 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.tInd_con = 0; % critical time index
+  SP.crit_pred = 0; % critical predicate
+  SP.constraints_tInd = zeros; % stores critical time indices values
+  SP.crit_predVal = zeros; % stores corresponding critical predicates
 
 
-  SP.break_loop = 0;
+  SP.break_loop = 0; 
 
-  prev_tInd_ind = [];
+  prev_tInd_ind = []; % tracks if a critical predicate at a certain time was added already ---> avoids looping 
 
 
   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.crit_predVal(prev_tInd_ind) == SP.crit_pred, 1))))  && ~SP.break_loop && toc(iter_time) < 0.5*SP.ds) % limit execution time to less that the time-step 
 
     SP.constraints_tInd(sol_count) = SP.tInd_con;
     SP.crit_predVal(sol_count) = SP.crit_pred;
 
+
+    % solve the optimization problem
     [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}};
 
 
+    % proceed if feasible
     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));
+    SP.yout(:,ii+1:end) = solutions{3}(:,2:end-(ii-1)); % append future output at the end of th path already executed
 
 
     %===========================================================================
     % Compute Robustness and identify the critical constraint
     %===========================================================================
 
-
+    % compute critical time, predicate and robustness
     [~, SP.tmin_con, SP.dmin_con, ~, SP.crit_pred] = staliro_distance(SP, ...
       SP.yout', SP.times');
 
+    % checks if critical time corresponds to a time that has already passed 
+    % abs(SP.dmin_con) > 1e-5 is to avoid looping due to numerical inaccuracies
 
     if SP.tmin_con > ((ii-1)*SP.ds) && abs(SP.dmin_con) ...
-        > 1e-5 && sign(SP.dmin_con) == -1
+        > 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);
@@ -144,7 +146,7 @@
   uvals(:,ii) = solutions{1}(:,1);
   yvals(:,ii) = solutions{3}(:,1);
 
-
+  % update position of the robot
   if plot_flag
 
     figure(hfig);

create_and_draw_car.m

@@ -1,8 +1,9 @@
 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
+%   z_n - Position vector of object
+%   R_n - Rotation matrix of object
+%   mode - Called in CREATE_CAR to scale the size of the object
+%   n - number of objects to be created
 
 %% Object : Car
 

create_car.m

@@ -0,0 +1,68 @@
+function obj = create_car(varargin)
+% CREATE_OBJECT - Creates visual representation of object to be controlled
+
+mode = varargin{1};
+if nargin == 1
+    scale_size = 1;
+else
+    scale_size = varargin{2};
+end
+
+if mode % for getting trajectory
+    obj.L = scale_size*(0.55+0.35)*2;            % Car length
+    obj.W = scale_size*(0.55+0.35)*2;            % Car width
+else % for representation purposes
+    obj.L = scale_size*0.55;            % Car length
+    obj.W = scale_size*0.55;            % Car width
+end
+
+obj.H = scale_size*0.25;            % Car height
+obj.CoM = [0;0;0];       % Center of Mass
+obj.gravity = 9.81;      % m-sec^2 - gravity
+obj.mass = 0.5;
+obj.r_wheel = scale_size*0.15;       % radius of the wheels
+
+
+obj.facs = [1 2 3 4
+    5 8 7 6
+    4 3 8 5
+    3 2 7 8
+    2 1 6 7
+    1 4 5 6];
+
+obj.coord.x = 0.5*obj.L*[-1 -1 1 1 1 -1 -1 1]';
+obj.coord.y = 0.5*obj.W*[-1 1 1 -1 -1 -1 1 1]';
+obj.coord.z = 0.5*obj.H*[1 1 1 1 -1 -1 -1 -1]';
+
+obj.vertices = [obj.coord.x...
+    obj.coord.y obj.coord.z]';
+
+obj.vertices = obj.vertices + ...
+    obj.CoM(:,ones(1,length(obj.vertices)));
+
+obj.friction = 0.2;
+
+obj.color = 'c';
+obj.alpha = 1;
+obj.u_max = [1.5 -1.5 1.5];
+
+res = 1/30; % Resolution for drawing wheels
+
+arm.wheels1 = [-obj.L/2  +  0*(0:res:1);
+    obj.W/4  +  obj.r_wheel*sin(2*pi*(0:res:1));
+    -obj.H/2    +  obj.r_wheel*cos(2*pi*(0:res:1))];
+
+arm.wheels2 = [obj.L/2  +  0*(0:res:1);
+    obj.W/4  +  obj.r_wheel*sin(2*pi*(0:res:1));
+    -obj.H/2    +  obj.r_wheel*cos(2*pi*(0:res:1))];
+
+for i = 1:2:4
+    obj.arm(i).wheels.coords = rot([0 0 1], (i-1)*pi/2) * arm.wheels1;
+end
+for i = 2:2:4
+    obj.arm(i).wheels.coords = rot([0 0 1], (i-2)*pi/2) * arm.wheels2;
+end
+
+obj.verts_init = obj.vertices;
+
+end

find_OptimalTraj_mpc_m3pi.m

@@ -16,10 +16,9 @@
 % set what is to be optimized
 obj_u = 1; % when set minimizes 1-norm of control signal
 
+ctrl_horizon = 1; % control horizon for MPC
 
-ctrl_horizon = 1;
-
-rob_ball_des = 0.05;
+rob_ball_des = 0.05; % desired robustness radius
 
 plot_flag = 1; % when set plots the position of the m3pi robot in each iteration
 
@@ -28,21 +27,22 @@
 %% Describe system
 
 SP = get_system_description();
-SP.t0 = 0;
-SP.ds = ds;
+SP.t0 = 0; % initial time
+SP.ds = ds; % time-step
+
+SP.pred_known_sz = []; % number of known predicates
+SP.pred_unknown_sz = []; % number of unknown predicates
 
-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;
+hfig = SP.hfig; % figure handle
 
 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;
+    SP.pred_num = SP.pred_known_sz + SP.pred_unknown_sz; % total number of predicates
 end
 
 SP.rad = 0.067/2; % size of the m3pi robot
@@ -51,19 +51,19 @@
 %% -------------------------- System Parameters --------------------------------
 
 
-SP.times = SP.t0:SP.ds:SP.tf;
+SP.times = SP.t0:SP.ds:SP.tf; % time-points 
 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.times*0.; % disturbance input signal
 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);
+SP.u0.input_values_human = zeros(SP.n_inputs, SP.input_length); %ignore this variable ---> needed for some other extensions of this work 
 
 % Initialize state variables
 
@@ -80,14 +80,14 @@
 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));
+SP.u0.min = SP.u0.min(:,ones(1,SP.hor_length+1)); % minimum control input ---> set in system description code
+SP.u0.max = SP.u0.max(:,ones(1,SP.hor_length+1)); % maximum control input
 
 %% ------------------------------ PLOTTING -------------------------------------
 
-SP.pred_actual = SP.pred;
-SP = modify_predicates_rob_ball( SP );
-SP.pred_orig = SP.pred;
+SP.pred_actual = SP.pred; % store the actual predicate size
+SP = modify_predicates_rob_ball( SP ); % shrink/bloat predicates depending on the robustness radius
+SP.pred_orig = SP.pred; 
 
 % Plot the predicates
 delete(hfig);

generate_critical_constraints_MILP.m

@@ -6,38 +6,34 @@
 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);
+SP.ss_cont= ss(SP.A, SP.B, SP.C, SP.D); % continuous dynamics
+SP.tf_disc = c2d(SP.ss_cont, SP.ds); % discrete dynamics
 
 %% 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);
+SP.u0.values = zeros(SP.n_inputs, SP.hor_length + 1); % optimized control signal values to be stored here
 
 %% Initialize other variables
-SP.t0 = 0;
-SP.tInd_con = 0;
-SP.crit_pred = 0;
-SP.phi = SP.phi_orig;
+SP.t0 = 0; % initial time
+SP.tInd_con = 0; % critical time index
+SP.crit_pred = 0; % critical predicate
+SP.phi = SP.phi_orig; % MTL specification
 
-SP.tmin_con = -1;
-SP.dmin_con = -Inf;
+SP.tmin_con = -1; % critical time
+SP.dmin_con = -Inf; % robustness radius
 
-SP.constraints_tInd = zeros;
-SP.crit_predVal = zeros;
+SP.constraints_tInd = zeros; % stores critical time indices values
+SP.crit_predVal = zeros; % stores corresponding critical predicates
 
-prev_tInd_ind = [];
-sol_count = 1;
+prev_tInd_ind = []; % tracks if a critical predicate at a certain time was added already ---> avoids looping 
+sol_count = 1; % keeps track of how many iterations were required to solve the problem
 
 
 % 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
 
@@ -54,10 +50,12 @@
   % Compute Robustness and identify the critical constraint
   %===========================================================================
 
+  % solve the optimization problem
   [solutions,diagnostics] = SP.controller{{x_cur(:) , SP.con_active , ...
     SP.hor_length+1, SP.u0.dist_input_values, SP.pred.A, SP.pred.b}};
 
 
+  % proceed if feasible
   if diagnostics == 1 || diagnostics == 12 || diagnostics == 15
     error('The problem is infeasible');
   end
@@ -68,7 +66,7 @@
   % Compute Robustness and identify the critical constraint
   %===========================================================================
 
-
+  % compute critical time, predicate and robustness
   [~, SP.tmin_con, SP.dmin_con, ~, SP.crit_pred] = staliro_distance(SP, ...
                                                   SP.yout_disc, SP.times');
 

get_system_description.m

@@ -1,9 +1,11 @@
 function SP = get_system_description()
+% SP = GET_SYSTEM_DESCRIPTION()
+% initializes unicycle robot dynamics
 
-  SP.u0.max = 0.25*[1 1]'; 
-  SP.u0.min = -0.25*[1 1]'; 
+  SP.u0.max = 0.25*[1 1]'; % max control allowed
+  SP.u0.min = -0.25*[1 1]'; % min control allowed
 
-  SP.A = [0 0 1 0;
+  SP.A = [0 0 1 0; 
     0 0 0 1;
     0 0 0 0;
     0 0 0 0];