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
modified files
Update RtdTrajOpt.m
Sukruthi-C committed Jul 23, 2023

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
commit 40264dd564733241903296411460053bb913b4f8
115 changes: 60 additions & 55 deletions REFINE_develop/NewhighwayAgentHelper.m
Original file line number Diff line number Diff line change
@@ -39,35 +39,40 @@
%dummy for testing
frs = 0;

AH@agentHelper(A,frs,varargin{:});
AH@agentHelper(A,frs,varargin{:}); %calls gen_paramter_standalone here
% options = struct();
% options.manu_type = 'speed change';
refinePlanner = Refine_Planner_test2(trajOptProps,t_plan);
AH.refinePlanner = refinePlanner;

AH.HLP = HLP;
AH.robotState = refinePlanner.robotstate;

AH.worldState = worldState;
AH.truncating_factor = 1;
refinePlanner = Refine_Planner(trajOptProps,t_plan,AH);
AH.refinePlanner = refinePlanner;
AH.robotState = refinePlanner.robotstate;
end



function [K,tout] = gen_parameter_standalone(AH, agent_state,waypoints)
function [trajectory,tout] = gen_parameter_standalone(AH, worldinfo,agent_state,waypoints)

%here we have our plan trajectory
[K,tout] = AH.refinePlanner.planTrajectory(AH.robotState,AH.worldState);
% disp(worldinfo)
disp('worldinfo class: ')
disp(class(worldinfo))
[trajectory,info] = AH.refinePlanner.planTrajectory(AH.robotState,worldinfo,AH.worldState);


if (AH.cur_t0_idx > 1 && AH.prev_action == 2) || (AH.cur_t0_idx > 2 && AH.prev_action == 3)|| AH.prev_action == 1
AH.prev_action = -1;
AH.cur_t0_idx = 1;
end

if AH.prev_action ~= -1
K = [AH.saved_K(1); AH.saved_K(2); AH.cur_t0_idx ;AH.prev_action];
AH.cur_t0_idx = AH.cur_t0_idx + 1;
return
end
%
% if (AH.cur_t0_idx > 1 && AH.prev_action == 2) || (AH.cur_t0_idx > 2 && AH.prev_action == 3)|| AH.prev_action == 1
% AH.prev_action = -1;
% AH.cur_t0_idx = 1;
% end
%
% if AH.prev_action ~= -1
% K = [AH.saved_K(1); AH.saved_K(2); AH.cur_t0_idx ;AH.prev_action];
% AH.cur_t0_idx = AH.cur_t0_idx + 1;
% return
% end

% x_des_mex = x_des;
% dyn_obs_mex = get_obs_mex(world_info.dyn_obstacles, world_info.bounds);
@@ -121,7 +126,7 @@
% AH.state_hist = [AH.state_hist agent_state];
% AH.time_hist = [AH.time_hist AH.A.time(end)];
% end
tout = 0; % place holder
% tout = 0; % place holder
end

function plot_selected_parameter_FRS(AH,K,type_manu,FRS,mirror_flag,agent_state,multiplier)
@@ -139,43 +144,43 @@ function plot_selected_parameter_FRS(AH,K,type_manu,FRS,mirror_flag,agent_state,



function [T, U, Z]=gen_ref(AH, K, reference_flag,agent_state, ref_time)
% generate reference based on parameter and states
if ~exist('agent_state','var')
agent_info = AH.get_agent_info();
agent_state = agent_info.state(:,end);
end
if ~exist('ref_time','var')
ref_time = AH.A.time(end);
end
if ~exist('reference_flag','var')
reference_flag = 1;
end
u_cur = agent_state(4) ;
y_cur = agent_state(2) ;
x_cur = agent_state(1) ;
Au = K(1);
Ay = K(2);
t0_idx = K(3);

t0 = (t0_idx-1)*AH.t_move;
type_manu = K(4);
if type_manu == 3 % 1: speed change. 2: direction change. 3: lane change
[T, U,Z] = gaussian_T_parameterized_traj_with_brake(t0,Ay,Au,u_cur,[],0,1);
else
[T, U,Z] = sin_one_hump_parameterized_traj_with_brake(t0,Ay,Au,u_cur,[],0,1);
end

if reference_flag
AH.ref_Z=[AH.ref_Z;x_cur+Z(1,:);y_cur+Z(2,:)];% for plotting
AH.t_real_start = [AH.t_real_start;ref_time];
else
AH.proposed_ref_Z=[AH.proposed_ref_Z;x_cur+Z(1,:);y_cur+Z(2,:)];% for plotting
AH.t_proposed_start = [AH.t_proposed_start;ref_time];
end


end
% function [T, U, Z]=gen_ref(AH, K, reference_flag,agent_state, ref_time)
% % generate reference based on parameter and states
% if ~exist('agent_state','var')
% agent_info = AH.get_agent_info();
% agent_state = agent_info.state(:,end);
% end
% if ~exist('ref_time','var')
% ref_time = AH.A.time(end);
% end
% if ~exist('reference_flag','var')
% reference_flag = 1;
% end
% u_cur = agent_state(4) ;
% y_cur = agent_state(2) ;
% x_cur = agent_state(1) ;
% Au = K(1);
% Ay = K(2);
% t0_idx = K(3);
%
% t0 = (t0_idx-1)*AH.t_move;
% type_manu = K(4);
% if type_manu == 3 % 1: speed change. 2: direction change. 3: lane change
% [T, U,Z] = gaussian_T_parameterized_traj_with_brake(t0,Ay,Au,u_cur,[],0,1);
% else
% [T, U,Z] = sin_one_hump_parameterized_traj_with_brake(t0,Ay,Au,u_cur,[],0,1);
% end
%
% if reference_flag
% AH.ref_Z=[AH.ref_Z;x_cur+Z(1,:);y_cur+Z(2,:)];% for plotting
% AH.t_real_start = [AH.t_real_start;ref_time];
% else
% AH.proposed_ref_Z=[AH.proposed_ref_Z;x_cur+Z(1,:);y_cur+Z(2,:)];% for plotting
% AH.t_proposed_start = [AH.t_proposed_start;ref_time];
% end
%
%
% end

function reset(AH,flags,eps_seed)
if ~exist('eps_seed','var')
82 changes: 29 additions & 53 deletions REFINE_develop/Refine_Planner.m
Original file line number Diff line number Diff line change
@@ -4,6 +4,7 @@
properties
manu_type %maneuver type
frs
rs
trajopt
trajOptProps
objective
@@ -17,17 +18,16 @@
methods (Static)
function options = defaultoptions()
options = struct();
% options.input_constraints_flag = true;%where is input getting passed from/why is it used?
% options.use_robust_input = true;%why do we need a robust input?
options.manu_type = 'speed change';
% options.verboseLevel = 'INFO';
end
end
methods
function self = Refine_Planner(trajOptProps,t_plan)%need options as argument
methods
function self = Refine_Planner(trajOptProps,t_plan,AH,options)%need options as argument with manu_type
arguments
trajOptProps (1,1) rtd.planner.trajopt.TrajOptProps
t_plan
AH
options struct
% options.manu_type {mustBeMember(options.manu_type,{'speed change','direction change','lane change'})}
end

@@ -36,83 +36,59 @@
waypoint1 = [10, 20]; % Example waypoint 1 [x1, y1]
waypoint2 = [15, 25]; % Example waypoint 2 [x2, y2]
waypoint3 = [20, 30]; % Example waypoint 3 [x3, y3]

% Store waypoints in a cell array/dummy variables
waypoint = {waypoint1, waypoint2, waypoint3};

% Initialize the properties
% self.manu_type = options.manu_type;
self.waypoint = waypoint;
self.trajOptProps = trajOptProps;

frs = FRS_loader_speed_change('converted_Au_frs.h5',t_plan);%check what all has to be passed here

file = 'converted_Au_frs.h5';
self.rs = struct;
self.rs.frs_ = FRS_loader_speed_change(file,t_plan);%check what all has to be passed here
% self.rs.AH =AH;
%using the t_plan find desired_idx for each re
frs = self.rs.frs_;
for i = 1:numel(frs.vehrs)
%frs should be of type struct
self.frs{i} = frs;

self.frs{i} = frs; %print frs
self.vehrs = frs.vehrs;
self.desired_idx = frs.desired_idx;
vehrs = self.vehrs;
Robotstate_ = frs.robotState(self.desired_idx ~=0);
state = Robotstate_{1}.state;
self.robotstate{i} = state;
%state property u,v,r
state_u = state.u;
state_v = state.v;
state_r = state.r;

%desired x,y,h
x_des = waypoint(:,1);
y_des = waypoint(:,2);
h_des = waypoint(:,3);


au = 2.0;
ay = 1.5;

%initial u0 value
u0 = state_u(1);

%taking the initial non zero positive number for h0 value
positive_idx = find(state.h >0);
non_zero_idx = positive_idx(state.h(positive_idx) ~=0);
h0 = state.h(non_zero_idx(1));

u0_goal = 5.0;
t0_offset = 0.2;

trajF = TrajectoryFactory();
trajectoryFactory = trajF.createTrajectory(au, ay, u0_goal, self.manu_type, u0, t0_offset, h0);
%TAKING FIRST TRAJECTORY HERE
self.trajectoryFactory = trajectoryFactory;
Robotstate_ = frs.robotState;
self.robotstate{i} = Robotstate_;
trajF = Trajectory_Factory(AH);

%Set up the optimization engine (optimizationEngine)
self.optimizationEngine = rtd.planner.trajopt.FminconOptimizationEngine(trajOptProps);

%objective function call
objective = Refine_Objective(trajOptProps,vehrs,self.desired_idx);

%trajectory optimization
self.trajopt{i} = rtd.planner.trajopt.RtdTrajOpt(trajOptProps, self.frs{i},objective, self.optimizationEngine, trajF);%pass the objective

self.trajopt_speed{i} = rtd.planner.trajopt.RtdTrajOpt(trajOptProps, self.rs,objective, self.optimizationEngine, trajF);%pass the objective
% self.trajopt_lane{i} = rtd.planner.trajopt.RtdTrajOpt(trajOptProps, self.rs,objective, self.optimizationEngine, trajF);
%take for speed change and lane change separetly
end


end

%plan trajectory function
function [trajectory,info] = planTrajectory(self,robotState,worldState)
function [trajectory,info] = planTrajectory(self,robotState,worldinfo,worldState)

frs_ = self.frs;

%generate frs instance-->generateReachableSet()
reachableSet = frs_.generateReachableSetWrapper(self, robotState, varargin);


for i = 1:numel(self.trajopt)
disp(robotState);
[trajectory, ~, info] = self.trajopt{i}.solveTrajOpt(robotState, worldState, self.waypoint,reachableSet);

%generate frs instance-->generateReachableSet()
frs_{i}.setWorldInfo(worldinfo);
[trajectory, ~, info] = self.trajopt_speed{i}.solveTrajOpt(robotState, worldState, self.waypoint);
% [trajectory, ~, info] = self.trajopt_lane{i}.solveTrajOpt(robotState, worldState, self.waypoint);
%best out of speed change and lane change.
end

end
end

end
end
3 changes: 0 additions & 3 deletions REFINE_develop/RtdTrajOpt.m
Original file line number Diff line number Diff line change
@@ -22,7 +22,6 @@
%
properties
trajOptProps rtd.planner.trajopt.TrajOptProps %
robot %
reachableSets %
objective %
optimizationEngine %
@@ -58,15 +57,13 @@
%
arguments
trajOptProps (1,1) rtd.planner.trajopt.TrajOptProps
% robot (1,1) rtd.sim.world.WorldEntity
reachableSets (1,1) struct
objective (1,1) rtd.planner.trajopt.Objective
optimizationEngine (1,1) rtd.planner.trajopt.OptimizationEngine
trajectoryFactory (1,1) rtd.planner.trajectory.TrajectoryFactory
options.verboseLevel (1,1) rtd.util.types.LogLevel = 'DEBUG'
end
self.trajOptProps = trajOptProps;
% self.robot = robot; % this might go
self.reachableSets = reachableSets;
self.objective = objective;
self.optimizationEngine = optimizationEngine;
70 changes: 70 additions & 0 deletions REFINE_develop/Trajectory_Factory.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
classdef Trajectory_Factory < rtd.planner.trajectory.TrajectoryFactory
properties
AH
end
methods

function self = Trajectory_Factory(AH)
self.AH = AH;
end

function traj = createTrajectory(self,robotState,rsInstance,parameter)

%the above vehrs and robotState is only of the desired idx
%au_values: longitudnal velocity ->parameter
%ay_values: lateral velocity
%u0_goal: goal longitudnal velocity
%manu_type_val: maneuver type
%t0_offset: time offset for a trajectory
%u0_val:initial longitudnal velocity of the robot
%h0_val: initial head of the robot

disp(class(parameter))
disp(parameter)
au = parameter(1);
ay = parameter(2);
u0_goal = 4; %dummy
manu_type = 'speed change';
u0 = 4;
t0_offset =0;
h0=0;

if nargin < 5
u0 = 0.0;
end
if nargin < 6
t0_offset = 0.0;
end
if nargin < 7
h0 = 0.0;
end

% Create an instance of the Trajectory class
K = [2,3,2,1];
agent_state = [4,5,3,5];
traj = Trajectory_reference(self.AH, K, agent_state);% AgentHelper, K=[Au,Ay,t0_idx,Manu_type], agent_state = [x_curr,y_curr,h_curr,u_curr]

% Set the trajectory parameters
traj.trajectoryParams = 0; % Specify the trajectory parameters here

% Set the trajectory optimization properties
% DUmmy variables for testing
trajOptProps = rtd.planner.trajopt.TrajOptProps();
trajOptProps.timeForCost = 1.0; % Set the time used for evaluation in the cost function
trajOptProps.planTime = 0.5; % Set the time duration of the nominal plan
trajOptProps.horizonTime = 1.0; % Set the time of the overall trajectory until stop
trajOptProps.doTimeout = false; % Set whether or not to timeout the optimization
trajOptProps.timeoutTime = 0.5; % Set the timeout time for the optimization
trajOptProps.randomInit = false; % Set whether or not to randomize unknown or extra parameters
traj.trajOptProps = trajOptProps; % Specify the trajectory optimization properties here

% Set the start state
traj.startState = rtd.entity.states.EntityState(); % Specify the start state here

% Call the setParameters method
traj.setParameters(au, ay, u0_goal, manu_type, u0, t0_offset, h0);
end
end


end
Loading