Initial Commit

- Add provided template code for control project
- Add control project documentation pdf

SimPkg_F21(student_ver)/forwardIntegrate.m

@@ -0,0 +1,116 @@
+function [Y,U,t_total,t_update] = forwardIntegrate()
+% [Y,U,t_total,t_update] = forwardIntegrate
+% 
+% This script returns the vehicle trajectory with control input being
+% generated via the control input generation function:
+%         ROB535_ControlProject_part2_Team<your team number>
+% Obstacles are randomly generated along the test track. Notice that the
+% vehicle can only sense (observe) the obstacles within 150m, therefore
+% the control input generation function is called repeatedly. In this
+% script, we assume the control input generation function is called every
+% 'dt' second (see line 32). 
+% 
+% OUTPUTS:
+%   Y         an N-by-6 vector where each column is the trajectory of the
+%             state of the vehicle 
+%   
+%   U         an N-by-2 vector of inputs, where the first column is the
+%             steering input in radians, and the second column is the
+%             longitudinal force in Newtons
+%   
+%   t_total   a scalar that records the total computational time  
+%   
+%   t_update  a M-by-1 vector of time that records the time consumption
+%             when the control input generation function is called
+%             
+% Written by: Jinsun Liu
+% Created: 31 Oct 2021
+
+
+    load('TestTrack.mat') % load test track
+
+    dt = 0.5; 
+    TOTAL_TIME = 20*60; % second
+    
+    % initialization
+    t_total = 0;
+    t_update = zeros(TOTAL_TIME/dt+1,1);
+    Y = zeros(TOTAL_TIME/0.01+1,6);
+    U = zeros(TOTAL_TIME/0.01,2);
+    Y(1,:) = [287,5,-176,0,2,0];
+
+    % generate obstacles along the track
+    Xobs = generateRandomObstacles(9 + randi(16),TestTrack);
+
+    iteration = 1; % a counter that counts how many times the control input 
+                   % generation function is called.
+
+    TIMER = tic; % start the timer
+    
+    % you only have TOTAL_TIME seconds to sense the obstacles, update
+    % control inputs, and simulate forward vehicle dynamcis.
+    while t_total < TOTAL_TIME 
+        curr_pos = Y( (iteration-1)*dt/0.01+1 , [1,3] ); % record current vehicle position
+        Xobs_seen = senseObstacles(curr_pos, Xobs); % sense the obstacles within 150m
+        curr_state = Y( (iteration-1)*dt/0.01+1 , : ); % record current vehicle states
+
+        
+        
+        % compute control inputs, and record the time consumption
+        t_temp = toc(TIMER);
+        %%%%%%%%%%%%%%%% THIS IS WHERE YOUR FUNCTION IS CALLED (replace in your team number). %%%%%%%%%%%%%%%%%%%%%%%%%%%
+        [Utemp, FLAG_terminate] = ROB535_ControlProject_part2_Team<your team number>(TestTrack,Xobs_seen,curr_state); %%
+        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%         FLAG_terminate = randi(2)-1                                 % GSIs: This line is just for us to debug. Feel free to play with it if you want
+%         Utemp = rand(dt/0.01+1 + FLAG_terminate* (randi(10)-5),2);  % GSIs: This line is just for us to debug. Feel free to play with it if you want        
+        t_update(iteration) = toc(TIMER)-t_temp;
+        
+        
+        
+        % Utemp must contain control inputs for at least dt second,
+        % otherwise stop the whole computation.
+        if size(Utemp,1)<dt/0.01+1 && FLAG_terminate == 0
+            fprintf('When FLAG_terminate = 0, Utemp cannot contain control inputs for less than %f second. \n',dt);
+            fprintf('Solving process is terminated.\n');
+            t_total = toc(TIMER);
+            break
+        end
+
+        
+        
+        if FLAG_terminate == 0
+            % if FLAG_terminate == 0, simulate forward vehicle dynamics for
+            % dt second.
+            U( (iteration-1)*dt/0.01+1:iteration*dt/0.01 , : ) = Utemp(1:dt/0.01,:);
+            Ytemp = forwardIntegrateControlInput( Utemp(1:dt/0.01+1,:) , curr_state );
+            Y( (iteration-1)*dt/0.01+2:iteration*dt/0.01+1 , : ) = Ytemp(2:end,:);
+        
+            % update the counter
+            iteration = iteration + 1;
+        else
+            % if FLAG_terminate == 1, simulate forward vehicle dynamics for
+            % no more than dt second, and stop the solving process.
+            simulate_length = min(dt/0.01+1, size(Utemp,1));
+            U((iteration-1)*dt/0.01+1:(iteration-1)*dt/0.01+simulate_length-1, :) = Utemp(1:simulate_length-1,:);
+            Ytemp = forwardIntegrateControlInput( Utemp(1:simulate_length,:) , curr_state );
+            Y((iteration-1)*dt/0.01+2:(iteration-1)*dt/0.01+simulate_length, : ) = Ytemp(2:end,:);
+        end
+        
+        
+        % update t_total
+        t_total = toc(TIMER);
+
+        % stop the computation if FLAG_terminate == 1
+        if FLAG_terminate == 1
+            break
+        end
+    end
+
+    % if reach the finish line before TOTAL_TIME, ignore any parts of the
+    % trajectory after crossing the finish line.
+    idx_temp = find(sum(abs(Y),2)==0,1);
+    Y(idx_temp:end,:) = [];
+    U(idx_temp:end,:) = [];
+    t_update(iteration:end) = [];
+
+end

SimPkg_F21(student_ver)/forwardIntegrateControlInput.m

@@ -0,0 +1,114 @@
+function [Y,T]=forwardIntegrateControlInput(U,x0)
+% function [Y] = forwardIntegrateControlInput(U,x0)
+% 
+% Given a set of inputs and an initial condition, returns the vehicles
+% trajectory. If no initial condition is specified the default for the track
+% is used.
+% 
+%  INPUTS:
+%    U           an N-by-2 vector of inputs, where the first column is the
+%                steering input in radians, and the second column is the 
+%                longitudinal force in Newtons.
+%    
+%    x0          a 1-by-6 vector of the initial state of the vehicle.
+%                If not specified, the default is used
+% 
+%  OUTPUTS:
+%    Y           an N-by-6 vector where each column is the trajectory of the
+%                state of the vehicle
+%
+%    T           a 1-by-N vector of time stamps for each of the rows of Y.
+% 
+%  Written by: Sean Vaskov
+%  Created: 31 Oct 2018
+%  Modified: 6 Nov 2018
+%
+%   Revision notes:
+%   - Sid Dey (6 Dec 2019)
+
+    %  if initial condition not given use default
+    if nargin < 2
+        x0 = [287,5,-176,0,2,0] ;
+    end
+
+    %generate time vector
+    T=0:0.01:(size(U,1)-1)*0.01;
+
+    if (length(T) == 1)
+        % in case U is a 1x2 vector, meaning T would be a 1x1 scalar, 
+        % we return the initial condition.
+        Y = x0;
+        
+    else
+        %Solve for trajectory
+        options = odeset('MaxStep',0.01);
+        [~,Y]=ode45(@(t,x)bike(t,x,T,U),T,x0,options);
+        
+        % in case U is a 2x2 vector, meaning T would be a 1x2 vector [t0 tf],
+        % ode45 would provide the solution at its own integration timesteps
+        % (in between t0 and tf). To avoid this, we simply take the first
+        % and last value of Y (which should correspond with t0 and tf).
+        
+        if ( size(U,1) ~= size(Y,1) )
+            % Take first and last value of Y.
+            Y = [Y(1, :); Y(end, :)];
+        end
+        
+    end
+    
+end
+
+function dzdt=bike(t,x,T,U)
+%constants
+Nw=2;
+f=0.01;
+Iz=2667;
+a=1.35;
+b=1.45;
+By=0.27;
+Cy=1.2;
+Dy=0.7;
+Ey=-1.6;
+Shy=0;
+Svy=0;
+m=1400;
+g=9.806;
+
+
+%generate input functions
+delta_f=interp1(T,U(:,1),t,'previous','extrap');
+F_x=interp1(T,U(:,2),t,'previous','extrap');
+
+%slip angle functions in degrees
+a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
+a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
+
+%Nonlinear Tire Dynamics
+phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
+phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
+
+F_zf=b/(a+b)*m*g;
+F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
+
+F_zr=a/(a+b)*m*g;
+F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
+
+F_total=sqrt((Nw*F_x)^2+(F_yr^2));
+F_max=0.7*m*g;
+
+if F_total>F_max
+    
+    F_x=F_max/F_total*F_x;
+  
+    F_yr=F_max/F_total*F_yr;
+end
+
+%vehicle dynamics
+dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
+          (-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
+          x(2)*sin(x(5))+x(4)*cos(x(5));...
+          (F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
+          x(6);...
+          (F_yf*a*cos(delta_f)-F_yr*b)/Iz];
+end
+

SimPkg_F21(student_ver)/generateRandomObstacles.m

@@ -0,0 +1,101 @@
+function Xobs = generateRandomObstacles(Nobs,TestTrack)
+% Xobs = generateRandomObstacles(Nobs)
+%
+% Given a number of obstacles Nobs and a track, place obstacles at random
+% orientations with one corner of each obstacle pinned to the center line
+% of the track
+%
+% INPUTS:
+%   Nobs        an integer defining the number of obstacles to output
+%
+%   TestTrack   a TestTrack object for which TestTrack.cline is the
+%               centerline
+%
+% OUTPUTS:
+%   Xobs        a 1-by-Nobs cell array, where each cell contains a single
+%               rectangular obstacle defined as a 4-by-2 matrix where the
+%               first column is the x coordinates and the second column is
+%               the y coordinates
+%
+% Written by: Shreyas Kousik
+% Created: 12 Nov 2017
+% Modified: 13 Nov 2017
+
+    if nargin < 2
+        loaded_file = load('TestTrack.mat') ;
+        TestTrack = loaded_file.TestTrack ;
+    end
+
+    if Nobs > 100
+    warning(['Number of obstacles is greater than 100! This is likely to ',...
+             'make the resulting course infeasible.'])
+    end
+
+    % get the center line and boundaries, but exclude the parts of the
+    % track that are close to the beginning and end
+    c = TestTrack.cline(:,4:end-4) ;
+    h = TestTrack.theta(:,4:end-4) ;
+
+    % get the cumulative and total distance along the centerline
+    dists_along_cline = cumsum([0, sqrt(diff(c(1,:)).^2 + diff(c(2,:)).^2)]) ;
+    total_dist_along_cline = dists_along_cline(end) ;
+
+    % create a vector of random distances between obstacles
+    min_dist_btwn_obs = 10 ; % meters
+    max_dist_btwn_obs = total_dist_along_cline / Nobs ; % also meters
+    dists_btwn_obs = (max_dist_btwn_obs-min_dist_btwn_obs).*rand(1,Nobs) + min_dist_btwn_obs ;
+    obs_start_dists = cumsum(dists_btwn_obs) ;
+
+    % scale up the distances between the obstacles to be along the whole length
+    % of the track (this means the min and max distanes between obstacles will
+    % increase, but this is a hack anyways, so hah)
+    end_pct = 0.1*rand(1) + 0.85 ;
+    obs_start_dists = obs_start_dists.*(end_pct*total_dist_along_cline./obs_start_dists(end)) ;
+    
+    % NOTE: The lines above are meant to encourage the track to be
+    % feasible, but there is never a 100% guarantee off feasibility
+
+    % get the start point and orientation of each obstacle
+    obs_start_x = interp1(dists_along_cline,c(1,:),obs_start_dists) ;
+    obs_start_y = interp1(dists_along_cline,c(2,:),obs_start_dists) ;
+    obs_heading = interp1(dists_along_cline,h,obs_start_dists) ;
+
+    % generate a random size and random side of the road for each obstacle;
+    % the parameters below work well for the COTA track segment that we are
+    % using in ROB 599
+    obs_min_length = 1 ;
+    obs_max_length = 4 ;
+    obs_min_width = 3 ; 
+    obs_max_width = 7 ;
+    obs_lengths = (obs_max_length - obs_min_length).*rand(1,Nobs) + obs_min_length ;
+    obs_widths = (obs_max_width - obs_min_width).*rand(1,Nobs) + obs_min_width ;
+    obs_sides = round(rand(1,Nobs)) ; % 0 is right, 1 is left
+
+    % from each start point, create a CCW contour defining a box that is
+    % pinned to the centerline at one corner
+    Xobs = cell(1,Nobs) ;
+    for idx = 1:Nobs
+        % create box
+        lidx = obs_lengths(idx) ;
+        widx = obs_widths(idx) ;
+        obs_box = [0, 0, lidx, lidx ;
+                   0, -widx, -widx, 0] ;
+
+        % if the box is on left side of track, shift it by +widx in its
+        % local y-direction
+        obs_box = obs_box + obs_sides(idx).*[zeros(1,4) ; widx.*ones(1,4)] ;
+
+        % rotate box to track orientation
+        hidx = obs_heading(idx) ;
+        Ridx = [cos(hidx) -sin(hidx) ; sin(hidx) cos(hidx)] ;
+        obs_box = Ridx*obs_box ;
+
+        % shift box to start point
+        xidx = obs_start_x(idx) ;
+        yidx = obs_start_y(idx) ;
+        obs_box = obs_box + repmat([xidx;yidx],1,4) ;
+
+        % fill in Xobs
+        Xobs{idx} = obs_box' ;
+    end
+end

SimPkg_F21(student_ver)/getTrajectoryInfo.m

@@ -0,0 +1,95 @@
+function info = getTrajectoryInfo(Y,U,Xobs,t_update,TestTrack)
+% info = getTrajectoryInfo(Y,U,Xobs)
+%
+% Given a trajectory, input, and a cell array of obstacles, return
+% information about whether or not the trajectory left the track or crashed
+% into any obstacles, and if the input limits were exceeded.
+%
+% NOTE: the trajectory is only for the vehicle's center of mass, so we
+% are not checking if the "corners" of the vehicle leave the track or hit
+% any obstacles.
+%
+% INPUTS:
+%   Y           an N-by-2 trajectory in the x and y coordinates of the
+%               vehicle's states, where the first column is x and the
+%                second column is y OR an N-by-6 trajectory in the full
+%               state, where the first column is x and the third column is
+%               y
+%
+%   U           an N-by-2 vector of inputs, where the first column is the
+%               steering angle and the second column is the rear wheel
+%               driving force
+%   
+%   Xobs        a 1-by-Nobs cell array where each cell contains a 4-by-2
+%               obstacle definition, as would be generated by the
+%               generateRandomObstacles function (this is an optional
+%               argument, so leave it out if your trajectory doesn't avoid
+%               any obstacles)
+%
+%   TestTrack   a TestTrack object for which TestTrack.cline is the
+%               centerline (this is an optional argument; the function will
+%               by default try to load the provided TestTrack.mat file)
+%
+%   t_update    a M-by-1 vector of time that records the time consumption
+%               when the control input generation function is called
+%
+% OUTPUTS:
+%   info        a struct containing the following catergories 
+%               
+%               info.Y : The trajectory given as an input argument to the 
+%               function.
+%
+%               info.U : The inputs given as an input argument to the
+%               function.
+%
+%               info.t_finished : Time in seconds when the finish line is 
+%               crossed. An empty vector is returned if finish line is not
+%               crossed.
+%
+%               info.t_end : Time in seconds at end of trajectory.
+%
+%               info.left_track_position : 2-by-1 vector with x and y 
+%               coordinates of location where vehicle first leaves the 
+%               track. An empty vector is returned if vehicle never leaves 
+%               the track.
+%
+%               info.left_track_time : Time in seconds when vehicle first 
+%               leaves the track. An empty vector is returned if vehicle 
+%               never leaves the track.
+%
+%               info.left_percent_of_track_completed : Percentage of the
+%               track completed before vehicle first leaves the track. An 
+%               empty vector is returned if vehicle never leaves the track.
+%
+%               info.crash_position : 2-by-1 vector with x and y
+%               coordinates of location where vehicle first hits an
+%               obstacle. An empty vector is returned if vehicle never hits
+%               an obstacle.
+%
+%               info.crash_time :  Time in seconds when vehicle first hits
+%               an obstacle. An empty vector is returned if vehicle never
+%               hits an obstacle.
+%
+%               info.crash_percent_of_track_completed : Percentage of the
+%               track completed before vehicle first hits an obstacle. An
+%               empty vector is returned if vehicle never hits an obstacle.
+%
+%               info.input_exceeded : 1-by-2 boolean with the first entry
+%               being true if constraints on input 1 are violated and entry
+%               2 being true if constraints on input 2 are violated.
+%
+%               info.percent_of_track_completed : Percentage of track
+%               complete prior to leaving the track, hitting an obstacle,
+%               or the control inputs end.
+%
+%               info.t_score : Score of computational time as 
+%                      info.t_finished + M * max(num_exceed_time_limit,0),
+%               where M(=10) is some penalty value, num_exceed_time_limit
+%               is the number of elements in t_update that are longer than
+%               0.5 second. An empty vector is returned if finish line is 
+%               not crossed.
+%
+% Written by: Shreyas Kousik and Matthew Porter
+% Created: 14 Dec 2017
+% Modified: 24 Oct 2018 
+% Modified: 1 Nov 2021 (Jinsun Liu)

SimPkg_F21(student_ver)/senseObstacles.m

@@ -0,0 +1,27 @@
+function Xobs_seen = senseObstacles(curr_pos, Xobs)
+% Xobs_seen = senseObstacles(curr_pos, Xobs)
+% 
+% Given the current vehicle position, sense the obstacles within 150m.
+% 
+% INPUTS:
+%   curr_pos   a 2-by-1 vector where the 1st and 2nd elements represent the
+%              x and y coordinates of the current vehicle position
+%   
+%   Xobs       a cell array generated by generateRandomObstacles.m
+%   
+% OUTPUTS:
+%   Xobs_seen  a cell array which contains all obstacles that are no 
+%              greater than 150m from the vehicle. Each cell has the same
+%              structure as the cells in Xobs.
+%   
+% Written by: Jinsun Liu
+% Created: 31 Oct 2021
+
+
+
+    
+    Xobs_mat = cell2mat(Xobs');
+    dist = (Xobs_mat(:,1) - curr_pos(1)).^2 + (Xobs_mat(:,2) - curr_pos(2)).^2;
+    idx = unique(ceil(find(dist<=150^2)/4));
+    Xobs_seen = {Xobs{idx}};
+end