From a37a1d62d3fb139138daeaafba8dcf9f51605cb4 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Fri, 26 Nov 2021 14:42:21 -0500 Subject: [PATCH] Working on Generating Trajectory Manually - Move provided files out of "SimPkg_F21(student_ver)" folder - Add `sravan_part_1.m` that generates segments of trajectory - Currently at 57.34% track completion --- .../TestTrack.mat => TestTrack.mat | Bin .../forwardIntegrate.m => forwardIntegrate.m | 0 ...lInput.m => forwardIntegrateControlInput.m | 0 ...omObstacles.m => generateRandomObstacles.m | 0 ...getTrajectoryInfo.m => getTrajectoryInfo.m | 0 ...getTrajectoryInfo.p => getTrajectoryInfo.p | Bin .../senseObstacles.m => senseObstacles.m | 0 sravan_part_1.m | 164 +++++++++++++++++- 8 files changed, 163 insertions(+), 1 deletion(-) rename SimPkg_F21(student_ver)/TestTrack.mat => TestTrack.mat (100%) rename SimPkg_F21(student_ver)/forwardIntegrate.m => forwardIntegrate.m (100%) rename SimPkg_F21(student_ver)/forwardIntegrateControlInput.m => forwardIntegrateControlInput.m (100%) rename SimPkg_F21(student_ver)/generateRandomObstacles.m => generateRandomObstacles.m (100%) rename SimPkg_F21(student_ver)/getTrajectoryInfo.m => getTrajectoryInfo.m (100%) rename SimPkg_F21(student_ver)/getTrajectoryInfo.p => getTrajectoryInfo.p (100%) rename SimPkg_F21(student_ver)/senseObstacles.m => senseObstacles.m (100%) diff --git a/SimPkg_F21(student_ver)/TestTrack.mat b/TestTrack.mat similarity index 100% rename from SimPkg_F21(student_ver)/TestTrack.mat rename to TestTrack.mat diff --git a/SimPkg_F21(student_ver)/forwardIntegrate.m b/forwardIntegrate.m similarity index 100% rename from SimPkg_F21(student_ver)/forwardIntegrate.m rename to forwardIntegrate.m diff --git a/SimPkg_F21(student_ver)/forwardIntegrateControlInput.m b/forwardIntegrateControlInput.m similarity index 100% rename from SimPkg_F21(student_ver)/forwardIntegrateControlInput.m rename to forwardIntegrateControlInput.m diff --git a/SimPkg_F21(student_ver)/generateRandomObstacles.m b/generateRandomObstacles.m similarity index 100% rename from SimPkg_F21(student_ver)/generateRandomObstacles.m rename to generateRandomObstacles.m diff --git a/SimPkg_F21(student_ver)/getTrajectoryInfo.m b/getTrajectoryInfo.m similarity index 100% rename from SimPkg_F21(student_ver)/getTrajectoryInfo.m rename to getTrajectoryInfo.m diff --git a/SimPkg_F21(student_ver)/getTrajectoryInfo.p b/getTrajectoryInfo.p similarity index 100% rename from SimPkg_F21(student_ver)/getTrajectoryInfo.p rename to getTrajectoryInfo.p diff --git a/SimPkg_F21(student_ver)/senseObstacles.m b/senseObstacles.m similarity index 100% rename from SimPkg_F21(student_ver)/senseObstacles.m rename to senseObstacles.m diff --git a/sravan_part_1.m b/sravan_part_1.m index c8eddfa..6fcc981 100644 --- a/sravan_part_1.m +++ b/sravan_part_1.m @@ -28,4 +28,166 @@ u_0 = 5; % meters/second y_0 = -176; % meters v_0 = 0; % meters/second psi_0 = 2; % radians -r_0 = 0; % radians/second \ No newline at end of file +r_0 = 0; % radians/second +state_0 = [x_0, u_0, y_0, v_0, psi_0, r_0]; + +load('TestTrack.mat') + +%% Trajectory Synthesis: Segment 1 +segment_num = 1; +num_pts(segment_num) = 6e2; +delta_vals(segment_num) = -0.004; +F_x_vals(segment_num) = 3900; +[Y,T,U] = simulate_segment(segment_num, [], [], [], num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 2 +segment_num = 2; +num_pts(segment_num) = 4e2; +delta_vals(segment_num) = -0.3; +F_x_vals(segment_num) = -2000; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 3 +segment_num = 3; +num_pts(segment_num) = 1e2; +delta_vals(segment_num) = -0.05; +F_x_vals(segment_num) = 0; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 4 +segment_num = 4; +num_pts(segment_num) = 7.5e2; +delta_vals(segment_num) = 0.0; +F_x_vals(segment_num) = 1000; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 5 +segment_num = 5; +num_pts(segment_num) = 3e2; +delta_vals(segment_num) = 0.3; +F_x_vals(segment_num) = -500; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 6 +segment_num = 6; +num_pts(segment_num) = 3.5e2; +delta_vals(segment_num) = -0.03; +F_x_vals(segment_num) = 1000; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 7 +segment_num = 7; +num_pts(segment_num) = 1e2; +delta_vals(segment_num) = -0.005; +F_x_vals(segment_num) = -1000; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 8 +segment_num = 8; +num_pts(segment_num) = 2e2; +delta_vals(segment_num) = 0.025; +F_x_vals(segment_num) = -750; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 9 +segment_num = 9; +num_pts(segment_num) = 2.5e2; +delta_vals(segment_num) = 0.5; +F_x_vals(segment_num) = -500; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 10 +segment_num = 10; +num_pts(segment_num) = 5e2; +delta_vals(segment_num) = -0.02; +F_x_vals(segment_num) = 0; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 11 +segment_num = 11; +num_pts(segment_num) = 2.5e2; +delta_vals(segment_num) = -0.05; +F_x_vals(segment_num) = 500; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 12 +segment_num = 12; +num_pts(segment_num) = 2e2; +delta_vals(segment_num) = -0.01; +F_x_vals(segment_num) = 5000; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 13 +segment_num = 13; +num_pts(segment_num) = 2e2; +delta_vals(segment_num) = -0.1; +F_x_vals(segment_num) = -2000; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 14 +segment_num = 14; +num_pts(segment_num) = 3e2; +delta_vals(segment_num) = 0.175; +F_x_vals(segment_num) = -2000; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 15 +segment_num = 15; +num_pts(segment_num) = 5e2; +delta_vals(segment_num) = -0.01; +F_x_vals(segment_num) = 1000; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +%% Trajectory Synthesis: Segment 16 +segment_num = 16; +num_pts(segment_num) = 5e2; +delta_vals(segment_num) = 0.05; +F_x_vals(segment_num) = 0; +[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0); + +% Figures +close all; +clc; + +figure(1) +hold on; +grid on; + +for i = 1:length(num_pts) + [start_idx, end_idx] = get_indices(i, num_pts); + plot(Y(start_idx:end_idx,1), Y(start_idx:end_idx,3), '-'); +end +plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '--r'); +plot(TestTrack.br(1,:), TestTrack.br(2,:), '--r'); +plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '-.g'); + +info = getTrajectoryInfo(Y,U) + +%% Functions +function [Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0) + [start_idx, end_idx] = get_indices(segment_num, num_pts); + + if segment_num ~= 1 + state_0 = Y(start_idx-1,:); + end + + delta = delta_vals(segment_num); + F_x = F_x_vals(segment_num); + + U_segment = [delta * ones(num_pts(segment_num),1), F_x * ones(num_pts(segment_num),1)]; + [Y_segment, T_segment] = forwardIntegrateControlInput(U_segment, state_0); + + Y(start_idx:end_idx,:) = Y_segment; + T(start_idx:end_idx) = T_segment; + U(start_idx:end_idx,:) = U_segment; +end + +function [start_idx, end_idx] = get_indices(segment_num, num_pts) + if segment_num == 1 + start_idx = 1; + end_idx = num_pts(segment_num); + else + start_idx = sum(num_pts(1:segment_num-1)) + 1; + end_idx = sum(num_pts(1:segment_num)); + end +end \ No newline at end of file