Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Frs scratch #12

Open
wants to merge 61 commits into
base: develop
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
61 commits
Select commit Hold shift + click to select a range
426789f
Add files via upload
Sukruthi-C Jun 1, 2023
b28ced7
Update FRS_Generator_speed_change.m
Sukruthi-C Jun 2, 2023
770245d
Update FRS_Instance_speed_change.m
Sukruthi-C Jun 2, 2023
545ddc3
Add files via upload
Sukruthi-C Jun 6, 2023
236a449
Add files via upload
Sukruthi-C Jun 8, 2023
432e36c
Update Trajectory.m
Sukruthi-C Jun 11, 2023
64e032a
Add files via upload
Sukruthi-C Jun 11, 2023
df6f6e0
Update FRS_Generator_speed_change.m
Sukruthi-C Jun 16, 2023
f42044d
Update FRS_Instance_speed_change.m
Sukruthi-C Jun 16, 2023
477aea9
Add files via upload
Sukruthi-C Jun 16, 2023
3b2f529
Update cost_function.m
Sukruthi-C Jun 16, 2023
d2bbdd8
Update Trajectory.m
Sukruthi-C Jun 16, 2023
b7630c5
Update FRS_Generator_speed_change.m
Sukruthi-C Jun 18, 2023
7ccf3a1
Update FRS_Instance_speed_change.m
Sukruthi-C Jun 18, 2023
4aa0726
Update cost_function.m
Sukruthi-C Jun 27, 2023
e895450
Add files via upload
Sukruthi-C Jun 27, 2023
5131f94
Delete trajectory_check.m
Sukruthi-C Jun 27, 2023
1c82c0d
Add files via upload
Sukruthi-C Jun 27, 2023
a1b9939
Update and rename Refine_Planner_test2.m to Refine_Planner.m
Sukruthi-C Jun 27, 2023
c2b3a62
Update Refine_Planner_check.m
Sukruthi-C Jun 27, 2023
be91dab
Update Refine_Planner.m
Sukruthi-C Jun 27, 2023
715e64d
Delete FRS_Generator_speed_change.m
Sukruthi-C Jul 10, 2023
c15869e
Delete Consts.m
Sukruthi-C Jul 10, 2023
01c71d1
Delete FRS_Instance_speed_change.m
Sukruthi-C Jul 10, 2023
0776052
Delete Refine_Planner.m
Sukruthi-C Jul 10, 2023
1f1f562
Delete Refine_Planner_check.m
Sukruthi-C Jul 10, 2023
7337fe8
Delete Robot_State.m
Sukruthi-C Jul 10, 2023
195d5e9
Delete Trajectory.m
Sukruthi-C Jul 10, 2023
4e810e9
Delete TrajectoryFactory.m
Sukruthi-C Jul 10, 2023
3be70c3
Delete TrajectoryFactory_check.m
Sukruthi-C Jul 10, 2023
127857d
Delete cost_function.m
Sukruthi-C Jul 10, 2023
6bdb839
Delete frs_check.m
Sukruthi-C Jul 10, 2023
6a5f3c1
Commit message
Sukruthi-C Jul 10, 2023
fa10a26
Update and rename FRS_loader_speed_change_test2.m to FRS_loader_speed…
Sukruthi-C Jul 18, 2023
3ffadd9
Update and rename Refine_Planner_test2.m to Refine_Planner.m
Sukruthi-C Jul 18, 2023
7e4dae8
Update and rename test3_FRS_Instance_speed_change.m to FRS_Instance_s…
Sukruthi-C Jul 18, 2023
fcdf9ca
Update FRS_loader_speed_change.m
Sukruthi-C Jul 18, 2023
a3d896b
updates files
Sukruthi-C Jul 18, 2023
765a319
Update FRS_Instance_speed_change.m
Sukruthi-C Jul 18, 2023
6bf4482
Update Refine_Objective.m
Sukruthi-C Jul 19, 2023
1b2d6bc
Add files via upload
Sukruthi-C Jul 19, 2023
5dfdb7e
Update RtdTrajOpt.m
Sukruthi-C Jul 19, 2023
40264dd
modified files
Sukruthi-C Jul 19, 2023
afd040b
Merge branch 'frs_scratch' of github.com:roahmlab/rtd-code-architectu…
Sukruthi-C Jul 23, 2023
4f0386e
RtdTrajOpt updated
Sukruthi-C Jul 23, 2023
3fda6b4
Update FRS_Instance_speed_change.m
Sukruthi-C Jul 24, 2023
d28e727
new update
Sukruthi-C Jul 25, 2023
51acbdf
Finalizing merge after conflict resolution
Sukruthi-C Jul 25, 2023
87964f7
Update RtdTrajOpt.m
Sukruthi-C Jul 26, 2023
cbca88a
Code structure
Sukruthi-C Aug 2, 2023
d06e4ee
new changes
Sukruthi-C Aug 7, 2023
f3e55d2
agent commit work in progress
Sukruthi-C Aug 7, 2023
3313c4b
Update RefineAgentInfo.m
Sukruthi-C Aug 11, 2023
cad08d1
Update RefineAgentState.m
Sukruthi-C Aug 11, 2023
0debff3
Update RefinePatchVisual.m
Sukruthi-C Aug 11, 2023
16e732f
Update dynamics.m
Sukruthi-C Aug 11, 2023
aa1d7e2
Rename dynamics.m to RefineDynamics.m
Sukruthi-C Aug 11, 2023
397d857
Add files via upload
Sukruthi-C Aug 11, 2023
3cc6144
new_changes
Sukruthi-C Aug 25, 2023
08fc2bd
new
Sukruthi-C Aug 25, 2023
95afa71
project_setup_pdf
Sukruthi-C Aug 29, 2023
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Update FRS_Instance_speed_change.m
Sukruthi-C authored Jun 16, 2023

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
commit f42044dd819c9de67c4ece0eb0324bf407056177
175 changes: 123 additions & 52 deletions FRS_Instance_speed_change.m
Original file line number Diff line number Diff line change
@@ -1,68 +1,139 @@
classdef FRS_Instance_speed_change < rtd.planner.reachsets.ReachSetInstance & refine.Offline_Reachability_Analysis.vehicle_dynamics_generation
% Similar to FOInstance
% This is just an individual instance of FRS speed change
% generation.


classdef FRS_Instance_speed_change
properties
input_range = [-1.0, 1.0]
num_parameters = 0

% properties carried over from the original implementation
sliceable %info about the slicing, dimension to slice along,x,y,h dimensions --> p_u
%slicedinfo %result of slice operation, sliced values along x,y,h dimension, sum along each dimension
zonoSliceInfo %o/p of sliced operation & vector of sliceable values
Vehrs %a single reachable set consisting of xy center points, heading, zono gen x,y,h, no of gen of each zono, zonoslice info for each zono, t and others
%frsSelectInfo %info of specific frs, maneuver type, index, if frs is mirrored
frsMega % sets of frs for diff maneuvers and additional info
frsTotal %total frs and other parameters together
Vehrs
constraints
end

methods
function self = FRS_Instance_speed_change( ...
sliceable,sliceableinfo,zonoSLiceInfo,Vehrs,frsMega,frsTotal,brakeidx1,brakeidx2 ...
)
self.sliceable = sliceable;
%self.slicedinfo = sliceableinfo;
self.zonoSliceInfo = zonoSLiceInfo;
self.Vehrs = Vehrs;
%self.frsSelectInfo = frsSelectInfo;
self.frsMega = frsMega;
self.frsTotal = frsTotal;
function self = test2_FRS_Instance_speed_change(Vehrs)

self.Vehrs = Vehrs;
self.constraints = [];
Agent = agent();%dummy values for testing only
agent_info = Agent.get_agent_info();
W = dynamic_car_world('start_line', 5, 'bounds', [-5 5 0 12], 'N_obstacles', 2);%passing dummy variables
world_info = W.get_world_info(agent_info);
self.constraints = genNLConstraint(self,world_info);
end

% Handles the obstacle-frs pair or similar to generate the
% nlconstraint.
% Returns a function handle for the nlconstraint generated
% where the function's return type is [c, ceq, gc, gceq]
function nlconFunction = genNLConstraint(self, worldState)

sliceable = self.sliceable;

function constraints = genNLConstraint(self, worldState)
vehrs = self.Vehrs;
%check if the speed is btw the max and min speed
speed_constraint = @(x) speedConstraint(x,sliceable,vehrs);%check with z0vel
nlconFunction = @(x) deal(speed_constraint(x), []); %to have multiple instances.

num_zonotopes = numel(vehrs);
constraints = cell(1, num_zonotopes);

for each_zono = 1:num_zonotopes
zono = vehrs{each_zono};
obs_info = get_obs_mex(self,worldState.dyn_obstacles,worldState.bounds);
constraints{each_zono} = self.GenerateConstraints(zono,obs_info);
end
end

function [c,ceq] = speedConstraint(x,sliceable,vehrs)
%calculate the speed
speed = x(1); %needs to be passed
speed_min = sliceable(1);
speed_max = sliceable(2);

%check if the speed is between the limit
if(speed<speed_min)
c = speed - speed_min; %add c
elseif (speed>speed_max)
c = speed_max - speed; %subtract c
else
c=0;
end
ceq = []; %equality constraint


end

methods (Access = private)

%generate the obs_info
function obj_mex = get_obs_mex(self,dyn_obs, bounds)
if ~iscell(dyn_obs)
dyn_obs = {dyn_obs};
end

if numel(dyn_obs) < 2
error('Invalid dyn_obs format. Expected at least 2 elements.');
end
all_pts = dyn_obs{1};
all_vels = dyn_obs{2};
obj_mex = [];
n_obs = length(all_vels);
for dyn_obs_idx = 1:n_obs
dyn_obs_pts_start_idx = ((dyn_obs_idx-1) * 6) + 1;
curr_pts = all_pts(:,dyn_obs_pts_start_idx:dyn_obs_pts_start_idx+3);
deltas = max(curr_pts,[],2) - min(curr_pts,[],2);
means = mean(curr_pts, 2);
dyn_c_x = means(1);
dyn_c_y = means(2);
dyn_length = deltas(1);
dyn_width = deltas(2);
dyn_velocity = all_vels(dyn_obs_idx);
dyn_heading_rad = 0;
obj_mex(:,end+1) = [dyn_c_x;
dyn_c_y;
dyn_heading_rad;
dyn_velocity;
dyn_length;
dyn_width];
end
xlo = bounds(1) ; xhi = bounds(2) ;
ylo = bounds(3) ; yhi = bounds(4) ;
dx = xhi - xlo;
dy = yhi - ylo;
x_c = mean([xlo, xhi]);
y_c = mean([ylo, yhi]);
b_thick = 0.01;
b_half_thick = b_thick / 2.0;

% Top
obj_mex(:,end+1) = [x_c; yhi+b_half_thick; 0; 0; dx; b_thick];
% Bottom
obj_mex(:,end+1) = [x_c; ylo-b_half_thick; 0; 0; dx; b_thick];
% Right
obj_mex(:,end+1) = [xhi+b_half_thick; y_c; 0; 0; b_thick; dy];
% Left
obj_mex(:,end+1) = [xlo-b_half_thick; y_c; 0; 0; b_thick; dy];
end


%Generate constraints for vehrs
function constraints = GenerateConstraints(self, zono, obs_info)
num_obs_gens = 2;
num_out_zono_gens = numel(zono.generatorLength) + 2;

if size(obs_info, 2) < 1 || num_out_zono_gens < 1
constraints = Constraints();
return;
end

total_d_size = num_out_zono_gens;
total_c_size = 2 * num_out_zono_gens;
delta_d_arr = zeros(1, total_d_size);
c_arr = zeros(1, total_c_size);

% Compute initial values.
max_r_without_obs = num_out_zono_gens - num_obs_gens;

% delta_d(r, c) = abs(C(r,:) * G(:,c))
for r = 1:max_r_without_obs
delta_d_arr(r) = 0;

% C(r,:) = Normalize([-G(1, r); G(2, r)]), one-indexed
G = zono.generators;
c_r0 = -G(1,r);
c_r1 = G(2,r);
norm_factor = norm([c_r0, c_r1]);
c_r0 = c_r0 / norm_factor;
c_r1 = c_r1 / norm_factor;

% Save to C array
c_arr(2*r) = c_r0;
c_arr(2*r-1) = c_r1;

for c = 1:num_out_zono_gens - num_obs_gens
g_c0 = G(2,c);
g_c1 = G(1,c);
delta_d_arr(r) = delta_d_arr(r) + abs((c_r0 * g_c0) + (c_r1 * g_c1));
end
end

constraints.delta_d_arr_ = delta_d_arr;
constraints.c_arr_ = c_arr;
end

end
end
end