commit 0d1791887a8645b133eb4e43d2090a7aa5f5e327 Author: Sravan Balaji Date: Mon Nov 8 17:01:23 2021 -0500 Initial Commit - Add provided template code for control project - Add control project documentation pdf diff --git a/ROB535_Control_Project.pdf b/ROB535_Control_Project.pdf new file mode 100644 index 0000000..718ae97 Binary files /dev/null and b/ROB535_Control_Project.pdf differ diff --git a/SimPkg_F21(student_ver)/TestTrack.mat b/SimPkg_F21(student_ver)/TestTrack.mat new file mode 100644 index 0000000..7c4ba0f Binary files /dev/null and b/SimPkg_F21(student_ver)/TestTrack.mat differ diff --git a/SimPkg_F21(student_ver)/forwardIntegrate.m b/SimPkg_F21(student_ver)/forwardIntegrate.m new file mode 100644 index 0000000..d14e890 --- /dev/null +++ b/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 +% 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(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)
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 + diff --git a/SimPkg_F21(student_ver)/generateRandomObstacles.m b/SimPkg_F21(student_ver)/generateRandomObstacles.m new file mode 100644 index 0000000..cd41ae5 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/SimPkg_F21(student_ver)/getTrajectoryInfo.m b/SimPkg_F21(student_ver)/getTrajectoryInfo.m new file mode 100644 index 0000000..f81157e --- /dev/null +++ b/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) diff --git a/SimPkg_F21(student_ver)/getTrajectoryInfo.p b/SimPkg_F21(student_ver)/getTrajectoryInfo.p new file mode 100644 index 0000000..9cd2f46 Binary files /dev/null and b/SimPkg_F21(student_ver)/getTrajectoryInfo.p differ diff --git a/SimPkg_F21(student_ver)/senseObstacles.m b/SimPkg_F21(student_ver)/senseObstacles.m new file mode 100644 index 0000000..8b3dfb7 --- /dev/null +++ b/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 \ No newline at end of file