sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-17 16:42:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Re-Organize Files Again

Browse Source 
- Deliverables folder holds final files for submission
- Experimentation holds scripts and things related to our development of
  a final solution
- Simulation holds the provided files
- Added template for part 2 submission with comments
This commit is contained in:
Sravan Balaji
2021-11-30 11:17:24 -05:00
parent b0ad7defd3
commit 33945f6414
15 changed files with 40 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
Simulation/TestTrack.mat Normal file
View File

Binary file not shown.

116
Simulation/forwardIntegrate.m Normal file
View File

@@ -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 %%%%%%%%%%%%%%%%%%%%%%%%%%%
[Utemp, FLAG_terminate] = ROB535_ControlProject_part2_Team3(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

114
Simulation/forwardIntegrateControlInput.m Normal file
View File

@@ -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

101
Simulation/generateRandomObstacles.m Normal file
View File

@@ -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

95
Simulation/getTrajectoryInfo.m Normal file
View File

@@ -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)

BIN
Simulation/getTrajectoryInfo.p Normal file
View File

Binary file not shown.

27
Simulation/senseObstacles.m Normal file
View File

@@ -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