├── Old Files ├── Car3D_Intersection │ ├── Simulation │ │ ├── V.png │ │ ├── dV_dt.png │ │ ├── aUb_Sim.png │ │ ├── aUb_Sim_ZoomIn.png │ │ ├── aUb_color_coded_trajectory.png │ │ ├── odefun_dubinsCar.m │ │ ├── eval_u.m │ │ └── Simulation.m │ └── Car3D_Intersection │ │ ├── beta.mat │ │ ├── Create_Fourway_Intersection │ │ ├── theta_diff.cpp │ │ ├── .Car_test.cpp.swp │ │ ├── def_extraArgs.cpp │ │ ├── add_pyramid.cpp │ │ ├── get_subvector.cpp │ │ ├── update_lane.cpp │ │ ├── add_lane.cpp │ │ ├── add_barrier.cpp │ │ ├── until.cpp │ │ └── add_sq.cpp ├── SingleRoad_Square │ ├── README │ ├── beta_fun.cpp │ ├── Square_alpha_beta.cpp │ ├── alpha_road.cpp │ ├── Road_alpha_beta.cpp │ ├── add_sq.cpp │ ├── alpha_sq.cpp │ ├── beta_road.cpp │ └── beta_sq.cpp ├── Car4D_Car1D_Intersection │ ├── theta_diff.cpp │ ├── def_extraArgs.cpp │ ├── add_lane.cpp │ ├── add_barrier.cpp │ ├── update_lane.cpp │ ├── get_subvector.cpp │ ├── until.cpp │ └── add_lane_disturbance.cpp └── AA290_work │ └── Turtlebot_Car4D_Car1D_Intersection │ ├── Car4D_Car1D_Intersection │ ├── theta_diff.cpp │ ├── add_sq.cpp │ ├── def_extraArgs.cpp │ ├── add_lane_plain.cpp │ ├── add_barrier.cpp │ ├── add_lane.cpp │ ├── update_lane.cpp │ ├── get_subvector.cpp │ ├── until.cpp │ └── add_lane_disturbance.cpp │ └── Simulation │ ├── odefun_dubinsCar.m │ ├── eval_u.m │ ├── RunData.m │ ├── eval_u_deriv.m │ ├── computeGradients_C.m │ ├── computeGradients_L.m │ ├── computeGradients_R.m │ └── computeGradients.m ├── MPC Controller ├── Overtake │ ├── Casadi-Python │ │ ├── setup.pyc │ │ └── test_supervisor.py │ └── Yalmip-Matlab │ │ ├── odefun_dubinsCar.m │ │ ├── PID_Controller.m │ │ ├── loadSingleOptimalStates.m │ │ ├── alpha_U_beta_Ext.m │ │ ├── mdlvar.m │ │ ├── ROS_Overtake.m │ │ ├── loadSolveParameters.m │ │ ├── eval_u.m │ │ ├── loadOptimalStates.m │ │ ├── MPC_video.m │ │ └── eval_u_deriv.m └── Intersection │ ├── Casadi-Python │ ├── setup.py │ └── test_supervisor.py │ └── Yalmip-Matlab │ ├── odefun_dubinsCar.m │ ├── eval_u.m │ ├── loadSingleOptimalStates.m │ ├── eval_valuefunction_for_PID_Controller.m │ ├── mdlvar.m │ ├── alpha_U_beta_Ext.m │ ├── PID_Controller.m │ ├── loadSolveParameters.m │ ├── eval_u_deriv.m │ ├── solve_nlp.m │ ├── loadOptimalStates.m │ └── MPC_video.m ├── Turtlebot_Car4D_Car1D_Overtake ├── Reachability Computation │ ├── theta_diff.cpp │ ├── add_sq.cpp │ ├── eventually.cpp │ ├── def_extraArgs.cpp │ ├── always.cpp │ ├── add_lane_frame.cpp │ ├── add_barrier.cpp │ ├── add_lane.cpp │ ├── computeGradient_C.cpp │ ├── update_lane.cpp │ ├── get_subvector.cpp │ ├── until.cpp │ ├── add_lane_alpha.cpp │ ├── add_lane_alpha_2.cpp │ ├── add_lane_beta_1.cpp │ ├── ComputeGradient_C.cpp │ ├── ComputeHyperplanceCoefficients.cpp │ ├── add_lane_beta_2.cpp │ ├── add_lane_beta_2bot.cpp │ ├── add_lane_beta_2top.cpp │ ├── add_lane_disturbance.cpp │ ├── ComputeCompatibleController.cpp │ └── add_lane_disturbance_v2.cpp ├── Numerical Simulation │ ├── odefun_dubinsCar.m │ ├── ROS_Overtake.m │ ├── eval_u.m │ ├── RunData.m │ ├── computeGradients_C.m │ ├── computeGradients_L.m │ └── computeGradients_R.m └── Hardware Experiment │ └── ROS_Overtake.m ├── Turtlebot_Car4D_Car1D_Intersection ├── Reachability Computation │ ├── theta_diff.cpp │ ├── add_sq.cpp │ ├── def_extraArgs.cpp │ ├── add_barrier.cpp │ ├── add_lane.cpp │ ├── update_lane.cpp │ ├── get_subvector.cpp │ ├── until.cpp │ ├── README │ └── add_lane_disturbance.cpp ├── Numerical Simulation │ ├── odefun_dubinsCar.m │ ├── eval_u_deriv.m │ ├── eval_u.m │ ├── ROS_Intersection.m │ ├── RunData.m │ ├── computeGradients_C.m │ ├── computeGradients_L.m │ ├── computeGradients_R.m │ └── computeGradients.m └── Hardware Experiment │ └── ROS_Intersection.m ├── .gitignore ├── Test Sample ├── eventually.cpp ├── def_extraArgs.cpp ├── always.cpp └── until.cpp ├── STL Functions ├── eventually.cpp ├── always.cpp └── until.cpp ├── LICENSE └── README.md /Old Files/Car3D_Intersection/Simulation/V.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/stlhj/HEAD/Old Files/Car3D_Intersection/Simulation/V.png -------------------------------------------------------------------------------- /MPC Controller/Overtake/Casadi-Python/setup.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/stlhj/HEAD/MPC Controller/Overtake/Casadi-Python/setup.pyc -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Simulation/dV_dt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/stlhj/HEAD/Old Files/Car3D_Intersection/Simulation/dV_dt.png -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Simulation/aUb_Sim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/stlhj/HEAD/Old Files/Car3D_Intersection/Simulation/aUb_Sim.png -------------------------------------------------------------------------------- /Old Files/SingleRoad_Square/README: -------------------------------------------------------------------------------- 1 | 3/5/3018 2 | This folder contains the files required to output a single lane with vehicle level set & a square level set. 3 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/beta.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/stlhj/HEAD/Old Files/Car3D_Intersection/Car3D_Intersection/beta.mat -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Simulation/aUb_Sim_ZoomIn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/stlhj/HEAD/Old Files/Car3D_Intersection/Simulation/aUb_Sim_ZoomIn.png -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Simulation/aUb_color_coded_trajectory.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/stlhj/HEAD/Old Files/Car3D_Intersection/Simulation/aUb_color_coded_trajectory.png -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/Create_Fourway_Intersection: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StanfordASL/stlhj/HEAD/Old Files/Car3D_Intersection/Car3D_Intersection/Create_Fourway_Intersection -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Simulation/odefun_dubinsCar.m: -------------------------------------------------------------------------------- 1 | function dydt = odefun_dubinsCar(t,y,u) 2 | V = 5; 3 | dydt = zeros(3,1); 4 | dydt(1) = V*cos(y(3)); 5 | dydt(2) = V*sin(y(3)); 6 | dydt(3) = u(1); -------------------------------------------------------------------------------- /Old Files/Car4D_Car1D_Intersection/theta_diff.cpp: -------------------------------------------------------------------------------- 1 | FLOAT_TYPE theta_diff( 2 | FLOAT_TYPE a, 3 | FLOAT_TYPE b) 4 | { 5 | FLOAT_TYPE mod = std::fmod(std::abs(a-b),(2.*M_PI)); 6 | if (mod>M_PI){ 7 | mod = 2.*M_PI - mod; 8 | } 9 | return mod; 10 | } 11 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/theta_diff.cpp: -------------------------------------------------------------------------------- 1 | FLOAT_TYPE theta_diff( 2 | FLOAT_TYPE a, 3 | FLOAT_TYPE b) 4 | { 5 | FLOAT_TYPE mod = std::fmod(std::abs(a-b),(2.*M_PI)); 6 | if (mod>M_PI){ 7 | mod = 2.*M_PI - mod; 8 | } 9 | return mod; 10 | } 11 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/theta_diff.cpp: -------------------------------------------------------------------------------- 1 | FLOAT_TYPE theta_diff( 2 | FLOAT_TYPE a, 3 | FLOAT_TYPE b) 4 | { 5 | FLOAT_TYPE mod = std::fmod(std::abs(a-b),(2.*M_PI)); 6 | if (mod>M_PI){ 7 | mod = 2.*M_PI - mod; 8 | } 9 | return mod; 10 | } 11 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Reachability Computation/theta_diff.cpp: -------------------------------------------------------------------------------- 1 | FLOAT_TYPE theta_diff( 2 | FLOAT_TYPE a, 3 | FLOAT_TYPE b) 4 | { 5 | FLOAT_TYPE mod = std::fmod(std::abs(a-b),(2.*M_PI)); 6 | if (mod>M_PI){ 7 | mod = 2.*M_PI - mod; 8 | } 9 | return mod; 10 | } 11 | -------------------------------------------------------------------------------- /MPC Controller/Intersection/Casadi-Python/setup.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def load_params(hf): 4 | 5 | T = 1 6 | dt = 0.1 7 | iters = int(T/dt) 8 | 9 | return {'min':hf['g/min'][0], 'max':hf['g/max'][0], 'N':hf['g/N'][0], 'T':T, 10 | 'dt':dt, 'iters':iters} 11 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Car4D_Car1D_Intersection/theta_diff.cpp: -------------------------------------------------------------------------------- 1 | FLOAT_TYPE theta_diff( 2 | FLOAT_TYPE a, 3 | FLOAT_TYPE b) 4 | { 5 | FLOAT_TYPE mod = std::fmod(std::abs(a-b),(2.*M_PI)); 6 | if (mod>M_PI){ 7 | mod = 2.*M_PI - mod; 8 | } 9 | return mod; 10 | } 11 | -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/odefun_dubinsCar.m: -------------------------------------------------------------------------------- 1 | function dydt = odefun_dubinsCar(t,y,u) 2 | % V = 5; 3 | % dydt = zeros(3,1); 4 | % dydt(1) = V*cos(y(3)); 5 | % dydt(2) = V*sin(y(3)); 6 | % dydt(3) = u; 7 | 8 | dydt = zeros(6,1); 9 | dydt(1) = y(4)*cos(y(3)); 10 | dydt(2) = y(4)*sin(y(3)); 11 | dydt(3) = u(1); 12 | dydt(4) = u(2); 13 | dydt(5) = u(3); 14 | dydt(6) = 1; -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/odefun_dubinsCar.m: -------------------------------------------------------------------------------- 1 | function dydt = odefun_dubinsCar(t,y,u) 2 | % V = 5; 3 | % dydt = zeros(3,1); 4 | % dydt(1) = V*cos(y(3)); 5 | % dydt(2) = V*sin(y(3)); 6 | % dydt(3) = u; 7 | 8 | dydt = zeros(5,1); 9 | dydt(1) = y(4)*cos(y(3)); 10 | dydt(2) = y(4)*sin(y(3)); 11 | dydt(3) = u(1); 12 | dydt(4) = u(2); 13 | dydt(5) = u(3); -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Numerical Simulation/odefun_dubinsCar.m: -------------------------------------------------------------------------------- 1 | function dydt = odefun_dubinsCar(t,y,u) 2 | % V = 5; 3 | % dydt = zeros(3,1); 4 | % dydt(1) = V*cos(y(3)); 5 | % dydt(2) = V*sin(y(3)); 6 | % dydt(3) = u; 7 | 8 | dydt = zeros(5,1); 9 | dydt(1) = y(4)*cos(y(3)); 10 | dydt(2) = y(4)*sin(y(3))-0.01; 11 | dydt(3) = u(1); 12 | dydt(4) = u(2); 13 | dydt(5) = u(3); -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/odefun_dubinsCar.m: -------------------------------------------------------------------------------- 1 | function dydt = odefun_dubinsCar(t,y,u) 2 | % V = 5; 3 | % dydt = zeros(3,1); 4 | % dydt(1) = V*cos(y(3)); 5 | % dydt(2) = V*sin(y(3)); 6 | % dydt(3) = u; 7 | 8 | dydt = zeros(6,1); 9 | dydt(1) = y(4)*cos(y(3)); 10 | dydt(2) = y(4)*sin(y(3))-0.01; 11 | dydt(3) = u(1); 12 | dydt(4) = u(2); 13 | dydt(5) = u(3); 14 | dydt(6) = 1; -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Simulation/odefun_dubinsCar.m: -------------------------------------------------------------------------------- 1 | function dydt = odefun_dubinsCar(t,y,u) 2 | % V = 5; 3 | % dydt = zeros(3,1); 4 | % dydt(1) = V*cos(y(3)); 5 | % dydt(2) = V*sin(y(3)); 6 | % dydt(3) = u; 7 | 8 | dydt = zeros(5,1); 9 | dydt(1) = y(4)*cos(y(3)); 10 | dydt(2) = y(4)*sin(y(3)); 11 | dydt(3) = u(1); 12 | dydt(4) = u(2); 13 | dydt(5) = u(3); -------------------------------------------------------------------------------- /Old Files/SingleRoad_Square/beta_fun.cpp: -------------------------------------------------------------------------------- 1 | void beta_fun( 2 | beacls::FloatVec& beta, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE beta_offset){ 5 | // beta = (x0-beta_offset)^2 + (x1-beta_offset)^2 6 | std::transform(xs.cbegin(), xs.cend(), beta.begin(), beta.begin(), 7 | [beta_offset](const auto &xs_i, const auto &beta_i) { 8 | return beta_i - std::pow((xs_i - beta_offset), 2); }); 9 | 10 | } 11 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | code/data 2 | code/figs 3 | 4 | # Prerequisites 5 | *.d 6 | 7 | # Compiled Object files 8 | *.slo 9 | *.lo 10 | *.o 11 | *.obj 12 | 13 | # Precompiled Headers 14 | *.gch 15 | *.pch 16 | 17 | # Compiled Dynamic libraries 18 | *.so 19 | *.dylib 20 | *.dll 21 | 22 | # Fortran module files 23 | *.mod 24 | *.smod 25 | 26 | # Compiled Static libraries 27 | *.lai 28 | *.la 29 | *.a 30 | *.lib 31 | 32 | # Executables 33 | *.exe 34 | *.out 35 | *.app 36 | -------------------------------------------------------------------------------- /Test Sample/eventually.cpp: -------------------------------------------------------------------------------- 1 | int eventually( 2 | std::vector& datas, 3 | beacls::FloatVec beta, 4 | FLOAT_TYPE tau1, 5 | FLOAT_TYPE tau2, 6 | helperOC::DynSysSchemeData* schemeData, 7 | beacls::FloatVec tau, 8 | helperOC::HJIPDE_extraArgs extraArgs){ 9 | 10 | const size_t numel = schemeData->get_grid()->get_numel(); 11 | 12 | beacls::FloatVec alpha; 13 | alpha.assign(numel, 100.); 14 | 15 | int result = until(datas, alpha, beta, tau1, tau2, schemeData, tau, 16 | extraArgs); 17 | } 18 | -------------------------------------------------------------------------------- /STL Functions/eventually.cpp: -------------------------------------------------------------------------------- 1 | int eventually( 2 | std::vector& datas, 3 | beacls::FloatVec beta, 4 | FLOAT_TYPE tau1, 5 | FLOAT_TYPE tau2, 6 | helperOC::DynSysSchemeData* schemeData, 7 | beacls::FloatVec tau, 8 | helperOC::HJIPDE_extraArgs extraArgs){ 9 | 10 | const size_t numel = schemeData->get_grid()->get_numel(); 11 | 12 | beacls::FloatVec alpha; 13 | alpha.assign(numel, 100.); 14 | 15 | int result = until(datas, alpha, beta, tau1, tau2, schemeData, tau, 16 | extraArgs); 17 | } -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/eval_u.m: -------------------------------------------------------------------------------- 1 | function value = eval_u(state,g,value_function) 2 | %Linear interpolation 3 | 4 | [xd,yd,thd,Vd,y2d] = ndgrid(linspace(g.min(1),g.max(1),g.N(1)),linspace(g.min(2),g.max(2),g.N(2)),... 5 | linspace(g.min(3),g.max(3),g.N(3)),linspace(g.min(4),g.max(4),g.N(4)),linspace(g.min(5),g.max(5),g.N(5))); 6 | 7 | [xq,yq,thq,Vq,y2q] = ndgrid(state(1),state(2),state(3),state(4),state(5)); 8 | 9 | value = interpn(xd,yd,thd,Vd,y2d,value_function,xq,yq,thq,Vq,y2q); 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/PID_Controller.m: -------------------------------------------------------------------------------- 1 | function u = PID_Controller(th,V,J_th,J_V,dt,v_mat,th_mat) 2 | [opt_th_I,~] = max(J_th); 3 | [opt_V_I,~] = max(J_V); 4 | 5 | opt_th = th_mat(opt_th_I); 6 | opt_V = v_mat(opt_V_I); 7 | 8 | error_th = opt_th-th; 9 | error_V = opt_V-V; 10 | 11 | prop = zeros(2,1); 12 | prop(1) = Kp_th*error_th; 13 | prop(2) = Kp_V*error_V; 14 | 15 | integral(1) = integral(1) + error_th*dt; 16 | integral(2) = integral(2) + error_V*dt; 17 | 18 | u(1) = Kp_th*error_th + Ki_th*integral(1); 19 | u(2) = Kp_V*error_V + Ki_V*integral(2); 20 | 21 | end -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/loadSingleOptimalStates.m: -------------------------------------------------------------------------------- 1 | function single_OptStates = loadSingleOptimalStates(states,F,th_mat,v_mat) 2 | %%loadOptimalStates 3 | OptStates = []; 4 | % load the optimal heading and speed states from the value function 5 | 6 | %[xq,yq,thq,vq,y2q] = ndgrid(states(1),states(2),th_mat,states(4),states(5)); 7 | %J_v = ndgrid(states(1),states(2),states(3),v_mat,states(5)); 8 | J_th = F({states(1),states(2),th_mat,states(4),states(5),states(6)}); 9 | J_v = F({states(1),states(2),states(3),v_mat,states(5),states(6)}); 10 | 11 | [~,I_th] = max(J_th); 12 | [~,I_v] = max(J_v); 13 | 14 | single_OptStates(1) = th_mat(I_th); 15 | single_OptStates(2) = v_mat(I_v); 16 | 17 | end 18 | 19 | -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/loadSingleOptimalStates.m: -------------------------------------------------------------------------------- 1 | function single_OptStates = loadSingleOptimalStates(states,F,th_mat,v_mat) 2 | %%loadOptimalStates 3 | OptStates = []; 4 | % load the optimal heading and speed states from the value function 5 | 6 | %[xq,yq,thq,vq,y2q] = ndgrid(states(1),states(2),th_mat,states(4),states(5)); 7 | %J_v = ndgrid(states(1),states(2),states(3),v_mat,states(5)); 8 | J_th = F({states(1),states(2),th_mat,states(4),states(5),states(6)}); 9 | J_v = F({states(1),states(2),states(3),v_mat,states(5),states(6)}); 10 | 11 | [~,I_th] = max(J_th); 12 | [~,I_v] = max(J_v); 13 | 14 | single_OptStates(1) = th_mat(I_th); 15 | single_OptStates(2) = v_mat(I_v); 16 | 17 | end 18 | 19 | -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/alpha_U_beta_Ext.m: -------------------------------------------------------------------------------- 1 | clear all 2 | 3 | load overtake_output.mat 4 | overtake_output_Ext = cat(6,alpha_U_beta{:}); 5 | %clear alpha_U_beta1 alpha_U_beta2 6 | overtake_output_Ext = flip(overtake_output_Ext,6); 7 | 8 | overtake_output_Ext(:,:,:,:,:,end+1:end+2/0.25) = ... 9 | repmat(overtake_output_Ext(:,:,:,:,:,end),[1,1,1,1,1,2/0.25]); 10 | 11 | save('overtake_output_Ext.mat', 'overtake_output_Ext','g','-v7') 12 | 13 | 14 | 15 | %alpha_U_beta1_Ext(:,:,:,:,:,end+1:end+T_MPC_horizon/tstep) = repmat(alpha_U_beta1_Ext(:,:,:,:,:,end),[1,1,1,1,1,T_MPC_horizon/tstep]); 16 | %alpha_U_beta2_Ext(:,:,:,:,:,end+1:end+T_MPC_horizon/tstep) = repmat(alpha_U_beta2_Ext(:,:,:,:,:,end),[1,1,1,1,1,T_MPC_horizon/tstep]); -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/eval_valuefunction_for_PID_Controller.m: -------------------------------------------------------------------------------- 1 | function [Vq_th_mat,Vq_v_mat] = eval_valuefunction_for_PID_Controller(state,value_function,th_mat,v_mat,g) 2 | 3 | [xd,yd,thd,Vd,y2d] = ndgrid(linspace(g.min(1),g.max(1),g.N(1)),linspace(g.min(2),g.max(2),g.N(2)),... 4 | linspace(g.min(3),g.max(3),g.N(3)),linspace(g.min(4),g.max(4),g.N(4)),linspace(g.min(5),g.max(5),g.N(5))); 5 | 6 | [xq,yq,thq,Vq,y2q] = ndgrid(state(1),state(2),th_mat,state(4),state(5)); 7 | Vq_th_mat = interpn(xd,yd,thd,Vd,y2d,value_function,xq,yq,thq,Vq,y2q); 8 | 9 | [xq,yq,thq,Vq,y2q] = ndgrid(state(1),state(2),state(3),v_mat,state(5)); 10 | Vq_v_mat = interpn(xd,yd,thd,Vd,y2d,value_function,xq,yq,thq,Vq,y2q); 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/mdlvar.m: -------------------------------------------------------------------------------- 1 | classdef mdlvar < handle 2 | properties (SetAccess = private) 3 | variable 4 | const 5 | type 6 | end 7 | 8 | methods 9 | function obj = mdlvar(n,const,type) 10 | if nargin<3 11 | type = 'variable'; 12 | if nargin<2 13 | const = 1; 14 | end 15 | end 16 | obj.variable = sdpvar(1,n); 17 | obj.const = const; 18 | obj.type = type; 19 | end 20 | 21 | function out = physical(obj,subs) 22 | if nargin<2 23 | subs = 1:numel(obj.variable); 24 | end 25 | 26 | out = obj.variable(subs)*obj.const; 27 | end 28 | 29 | function out = value(obj) 30 | out = double(obj.variable)*obj.const; 31 | end 32 | 33 | function plot(obj) 34 | plot(value(obj)) 35 | end 36 | end 37 | end -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/mdlvar.m: -------------------------------------------------------------------------------- 1 | classdef mdlvar < handle 2 | properties (SetAccess = private) 3 | variable 4 | const 5 | type 6 | end 7 | 8 | methods 9 | function obj = mdlvar(n,const,type) 10 | if nargin<3 11 | type = 'variable'; 12 | if nargin<2 13 | const = 1; 14 | end 15 | end 16 | obj.variable = sdpvar(1,n); 17 | obj.const = const; 18 | obj.type = type; 19 | end 20 | 21 | function out = physical(obj,subs) 22 | if nargin<2 23 | subs = 1:numel(obj.variable); 24 | end 25 | 26 | out = obj.variable(subs)*obj.const; 27 | end 28 | 29 | function out = value(obj) 30 | out = double(obj.variable)*obj.const; 31 | end 32 | 33 | function plot(obj) 34 | plot(value(obj)) 35 | end 36 | end 37 | end -------------------------------------------------------------------------------- /Old Files/SingleRoad_Square/Square_alpha_beta.cpp: -------------------------------------------------------------------------------- 1 | #include "alpha_sq.cpp" 2 | #include "beta_sq.cpp" 3 | void Square_alpha_beta( 4 | beacls::FloatVec& alpha, 5 | beacls::FloatVec& beta, 6 | levelset::HJI_Grid* g) 7 | { 8 | 9 | FLOAT_TYPE alpha_offset = 0.; 10 | FLOAT_TYPE beta_offset = 0.; 11 | FLOAT_TYPE length = 4.; 12 | 13 | const size_t numel = g->get_numel(); 14 | const size_t num_dim = g->get_num_of_dimensions(); 15 | 16 | alpha.assign(numel, 0.); 17 | beta.assign(numel, 0.); 18 | 19 | for (size_t dim = 0; dim < num_dim; ++dim) { 20 | const beacls::FloatVec &xs = g->get_xs(dim); 21 | 22 | if (dim == 0 || dim == 1){ 23 | alpha_sq(alpha, xs, dim, numel, alpha_offset, length); 24 | beta_sq(beta, xs, dim, numel, beta_offset, length); 25 | } 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /Old Files/SingleRoad_Square/alpha_road.cpp: -------------------------------------------------------------------------------- 1 | void alpha_road( 2 | beacls::FloatVec& alpha, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE dim, 5 | const size_t numel, 6 | FLOAT_TYPE alpha_offset, 7 | FLOAT_TYPE theta_offset, 8 | FLOAT_TYPE vehicle_width, 9 | FLOAT_TYPE length){ 10 | 11 | if (dim == 0){ 12 | std::transform(xs.cbegin(), xs.cend(), alpha.begin(), 13 | [alpha_offset, length, vehicle_width](const auto &xs_i) { 14 | return 1- std::pow(((xs_i - alpha_offset)/(length-vehicle_width)),2); }); 15 | } 16 | else if (dim == 2) { 17 | std::transform(xs.cbegin(), xs.cend(), alpha.begin(), alpha.begin(), 18 | [theta_offset, length](const auto &xs_i, const auto &alpha_i) { 19 | return alpha_i - std::pow(((xs_i- theta_offset)/(M_PI)),2); }); 20 | } 21 | 22 | } 23 | -------------------------------------------------------------------------------- /Old Files/SingleRoad_Square/Road_alpha_beta.cpp: -------------------------------------------------------------------------------- 1 | #include "alpha_road.cpp" 2 | #include "beta_road.cpp" 3 | void Road_alpha_beta( 4 | beacls::FloatVec& alpha, 5 | beacls::FloatVec& beta, 6 | levelset::HJI_Grid* g) 7 | { 8 | 9 | FLOAT_TYPE alpha_offset = 0.; 10 | FLOAT_TYPE beta_offset = 0.; 11 | FLOAT_TYPE theta_offset = M_PI/2; 12 | FLOAT_TYPE vehicle_width = 0.5; 13 | FLOAT_TYPE length = 2.; 14 | 15 | const size_t numel = g->get_numel(); 16 | const size_t num_dim = g->get_num_of_dimensions(); 17 | alpha.assign(numel, 0.); 18 | beta.assign(numel, 0.); 19 | 20 | for (size_t dim = 0; dim < num_dim; ++dim) { 21 | const beacls::FloatVec &xs = g->get_xs(dim); 22 | 23 | if (dim == 0 || dim == 2){ 24 | alpha_road(alpha, xs, dim, numel, alpha_offset, theta_offset, vehicle_width, length); 25 | beta_road(beta, xs, dim, numel, beta_offset, length); 26 | } 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /Old Files/SingleRoad_Square/add_sq.cpp: -------------------------------------------------------------------------------- 1 | void add_sq( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE sq_offset, 5 | FLOAT_TYPE length, 6 | FLOAT_TYPE dim, 7 | const size_t numel){ 8 | 9 | if (dim == 0){ 10 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), 11 | [lane_offset, length](const auto &xs_i) { 12 | return 1- std::pow(((xs_i - sq_offset)/length),2); }); 13 | } 14 | else if (dim == 1) { 15 | beacls::FloatVec lane_temp; 16 | lane_temp.assign(numel, 0.); 17 | std::transform(xs.cbegin(), xs.cend(), lane_temp.begin(), 18 | [lane_offset, length](const auto &xs_i) { 19 | return 1- std::pow(((xs_i - sq_offset)/length),2); }); 20 | 21 | std::transform(lane_temp.cbegin(), lane_temp.cend(), lane.begin(), lane.begin(), 22 | [](const auto &lane_temp_i, const auto &lane_i) { 23 | return std::min(lane_temp_i,lane_i); }); 24 | } 25 | 26 | } 27 | -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/alpha_U_beta_Ext.m: -------------------------------------------------------------------------------- 1 | clear all 2 | 3 | load alpha_U_beta1.mat 4 | load alpha_U_beta2.mat 5 | alpha_U_beta1_Ext = cat(6,alpha_U_beta1{:}); 6 | alpha_U_beta2_Ext = cat(6,alpha_U_beta2{:}); 7 | clear alpha_U_beta1 alpha_U_beta2 8 | alpha_U_beta1_Ext = flip(alpha_U_beta1_Ext,6); 9 | alpha_U_beta2_Ext = flip(alpha_U_beta2_Ext,6); 10 | 11 | alpha_U_beta1_Ext(:,:,:,:,:,end+1:end+2/0.25) = ... 12 | repmat(alpha_U_beta1_Ext(:,:,:,:,:,end),[1,1,1,1,1,2/0.25]); 13 | 14 | alpha_U_beta2_Ext(:,:,:,:,:,end+1:end+2/0.25) = ... 15 | repmat(alpha_U_beta2_Ext(:,:,:,:,:,end),[1,1,1,1,1,2/0.25]); 16 | 17 | save('alpha_U_beta_Ext.mat') 18 | 19 | 20 | 21 | %alpha_U_beta1_Ext(:,:,:,:,:,end+1:end+T_MPC_horizon/tstep) = repmat(alpha_U_beta1_Ext(:,:,:,:,:,end),[1,1,1,1,1,T_MPC_horizon/tstep]); 22 | %alpha_U_beta2_Ext(:,:,:,:,:,end+1:end+T_MPC_horizon/tstep) = repmat(alpha_U_beta2_Ext(:,:,:,:,:,end),[1,1,1,1,1,T_MPC_horizon/tstep]); -------------------------------------------------------------------------------- /Old Files/SingleRoad_Square/alpha_sq.cpp: -------------------------------------------------------------------------------- 1 | void alpha_sq( 2 | beacls::FloatVec& alpha, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE dim, 5 | const size_t numel, 6 | FLOAT_TYPE alpha_offset, 7 | FLOAT_TYPE length){ 8 | 9 | if (dim == 0){ 10 | std::transform(xs.cbegin(), xs.cend(), alpha.begin(), 11 | [alpha_offset, length](const auto &xs_i) { 12 | return 1- std::pow(((xs_i - alpha_offset)/length),2); }); 13 | } 14 | else if (dim == 1) { 15 | beacls::FloatVec alpha_temp; 16 | alpha_temp.assign(numel, 0.); 17 | std::transform(xs.cbegin(), xs.cend(), alpha_temp.begin(), 18 | [alpha_offset, length](const auto &xs_i) { 19 | return 1- std::pow(((xs_i - alpha_offset)/length),2); }); 20 | 21 | std::transform(alpha_temp.cbegin(), alpha_temp.cend(), alpha.begin(), alpha.begin(), 22 | [](const auto &alpha_temp_i, const auto &alpha_i) { 23 | return std::min(alpha_temp_i,alpha_i); }); 24 | } 25 | 26 | } 27 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/.Car_test.cpp.swp: -------------------------------------------------------------------------------- 1 | b0nano 2.5.3qizhanqizhan-XPS-15-9550Car_test.cpp -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_sq.cpp: -------------------------------------------------------------------------------- 1 | void add_sq( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE xg, 5 | FLOAT_TYPE yg, 6 | FLOAT_TYPE x0, 7 | FLOAT_TYPE y0, 8 | size_t dim){ 9 | 10 | FLOAT_TYPE a = 3.; 11 | FLOAT_TYPE b = (a-2.)/(std::pow(x0-xg,2)+std::pow(y0-yg,2)); 12 | 13 | if (dim == 0){ 14 | //reset the default values 15 | std::fill(lane.begin(),lane.end(),0.); 16 | // for (int i = 0; i()); 27 | } 28 | 29 | } 30 | -------------------------------------------------------------------------------- /Old Files/SingleRoad_Square/beta_sq.cpp: -------------------------------------------------------------------------------- 1 | void beta_sq( 2 | beacls::FloatVec& beta, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE dim, 5 | const size_t numel, 6 | FLOAT_TYPE beta_offset, 7 | FLOAT_TYPE length){ 8 | 9 | if (dim == 0){ 10 | std::transform(xs.cbegin(), xs.cend(), beta.begin(), 11 | [beta_offset, length](const auto &xs_i) { 12 | return 1- std::pow(((xs_i - beta_offset)/length),2); }); 13 | } 14 | else if (dim == 1) { 15 | beacls::FloatVec beta_temp; 16 | beta_temp.assign(numel, 0.); 17 | std::transform(xs.cbegin(), xs.cend(), beta_temp.begin(), 18 | [beta_offset, length](const auto &xs_i) { 19 | return 1- std::pow(((xs_i - beta_offset)/length),2); }); 20 | 21 | std::transform(beta_temp.cbegin(), beta_temp.cend(), beta.begin(), beta.begin(), 22 | [](const auto &beta_temp_i, const auto &beta_i) { 23 | return std::min(beta_temp_i,beta_i); }); 24 | 25 | std::transform(beta.cbegin(), beta.cend(), beta.begin(), 26 | std::negate()); 27 | } 28 | 29 | } 30 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/eventually.cpp: -------------------------------------------------------------------------------- 1 | int eventually( 2 | std::vector& datas, 3 | beacls::FloatVec beta, 4 | FLOAT_TYPE tau1, 5 | FLOAT_TYPE tau2, 6 | helperOC::DynSysSchemeData* schemeData, 7 | beacls::FloatVec tau, 8 | helperOC::HJIPDE_extraArgs extraArgs){ 9 | 10 | const size_t numel = schemeData->get_grid()->get_numel(); 11 | 12 | beacls::FloatVec alpha; 13 | alpha.assign(numel, 100.); 14 | 15 | // const size_t num_dim = schemeData->get_grid()->get_num_of_dimensions(); 16 | // FLOAT_TYPE alpha_offset = 20.; 17 | // for (size_t dim = 0; dim < num_dim; ++dim) { 18 | // const beacls::FloatVec &xs = schemeData->get_grid()->get_xs(dim); 19 | 20 | // if (dim == 0) { // alpha = x0 - alpha offset 21 | // std::transform(xs.cbegin(), xs.cend(), alpha.begin(), 22 | // [alpha_offset](const auto &xs_i) { 23 | // return xs_i - alpha_offset; }); 24 | // } 25 | // } 26 | 27 | int result = until(datas, alpha, beta, tau1, tau2, schemeData, tau, 28 | extraArgs); 29 | } 30 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Car4D_Car1D_Intersection/add_sq.cpp: -------------------------------------------------------------------------------- 1 | void add_sq( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE xg, 5 | FLOAT_TYPE yg, 6 | FLOAT_TYPE x0, 7 | FLOAT_TYPE y0, 8 | size_t dim){ 9 | 10 | FLOAT_TYPE a = 3.; 11 | FLOAT_TYPE b = (a-2.)/(std::pow(x0-xg,2)+std::pow(y0-yg,2)); 12 | 13 | if (dim == 0){ 14 | //reset the default values 15 | std::fill(lane.begin(),lane.end(),0.); 16 | // for (int i = 0; iget_x()[2], p->get_x()[3]}; 14 | } 15 | else { 16 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0}; 17 | extraArgs.plotData.projpt = beacls::FloatVec{p->get_x()[2]}; 18 | } 19 | 20 | extraArgs.deleteLastPlot = true; 21 | extraArgs.fig_filename = "figs/Car_test"; 22 | 23 | extraArgs.execParameters.line_length_of_chunk = 1; 24 | extraArgs.execParameters.calcTTR = false; 25 | extraArgs.keepLast = false; 26 | extraArgs.execParameters.useCuda = false; 27 | extraArgs.execParameters.num_of_gpus = 0; 28 | extraArgs.execParameters.num_of_threads = 0; 29 | extraArgs.execParameters.delayedDerivMinMax = 30 | levelset::DelayedDerivMinMax_Disable; 31 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = true; 32 | 33 | return extraArgs; 34 | } 35 | 36 | -------------------------------------------------------------------------------- /Old Files/Car4D_Car1D_Intersection/def_extraArgs.cpp: -------------------------------------------------------------------------------- 1 | helperOC::HJIPDE_extraArgs def_extraArgs( 2 | bool accel, 3 | helperOC::DynSys* p) { 4 | 5 | helperOC::HJIPDE_extraArgs extraArgs; 6 | 7 | // Target set and visualization 8 | extraArgs.visualize = true; 9 | 10 | if (accel) { 11 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0, 0, 0}; 12 | extraArgs.plotData.projpt = 13 | beacls::FloatVec{p->get_x()[2], p->get_x()[3], p->get_x()[4]}; 14 | } 15 | else { 16 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0}; 17 | extraArgs.plotData.projpt = beacls::FloatVec{p->get_x()[2]}; 18 | } 19 | 20 | extraArgs.deleteLastPlot = true; 21 | extraArgs.fig_filename = "figs/Car_test"; 22 | 23 | extraArgs.execParameters.line_length_of_chunk = 1; 24 | extraArgs.execParameters.calcTTR = false; 25 | extraArgs.keepLast = false; 26 | extraArgs.execParameters.useCuda = false; 27 | extraArgs.execParameters.num_of_gpus = 0; 28 | extraArgs.execParameters.num_of_threads = 0; 29 | extraArgs.execParameters.delayedDerivMinMax = 30 | levelset::DelayedDerivMinMax_Disable; 31 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = true; 32 | 33 | return extraArgs; 34 | } 35 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/def_extraArgs.cpp: -------------------------------------------------------------------------------- 1 | helperOC::HJIPDE_extraArgs def_extraArgs( 2 | bool accel, 3 | helperOC::DynSys* p) { 4 | 5 | helperOC::HJIPDE_extraArgs extraArgs; 6 | 7 | // Target set and visualization 8 | extraArgs.visualize = true; 9 | 10 | if (accel) { 11 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0, 0, 0}; 12 | extraArgs.plotData.projpt = 13 | beacls::FloatVec{p->get_x()[2], p->get_x()[3], p->get_x()[4]}; 14 | } 15 | else { 16 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0}; 17 | extraArgs.plotData.projpt = beacls::FloatVec{p->get_x()[2]}; 18 | } 19 | 20 | extraArgs.deleteLastPlot = true; 21 | extraArgs.fig_filename = "figs/Car_test"; 22 | 23 | extraArgs.execParameters.line_length_of_chunk = 1; 24 | extraArgs.execParameters.calcTTR = false; 25 | extraArgs.keepLast = false; 26 | extraArgs.execParameters.useCuda = false; 27 | extraArgs.execParameters.num_of_gpus = 0; 28 | extraArgs.execParameters.num_of_threads = 0; 29 | extraArgs.execParameters.delayedDerivMinMax = 30 | levelset::DelayedDerivMinMax_Disable; 31 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = true; 32 | 33 | return extraArgs; 34 | } 35 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/def_extraArgs.cpp: -------------------------------------------------------------------------------- 1 | helperOC::HJIPDE_extraArgs def_extraArgs( 2 | bool accel, 3 | helperOC::DynSys* p) { 4 | 5 | helperOC::HJIPDE_extraArgs extraArgs; 6 | 7 | // Target set and visualization 8 | extraArgs.visualize = true; 9 | 10 | if (accel) { 11 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0, 0, 0}; 12 | extraArgs.plotData.projpt = 13 | beacls::FloatVec{p->get_x()[2], p->get_x()[3], p->get_x()[4]}; 14 | } 15 | else { 16 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0}; 17 | extraArgs.plotData.projpt = beacls::FloatVec{p->get_x()[2]}; 18 | } 19 | 20 | extraArgs.deleteLastPlot = true; 21 | extraArgs.fig_filename = "figs/Car_test"; 22 | 23 | extraArgs.execParameters.line_length_of_chunk = 1; 24 | extraArgs.execParameters.calcTTR = false; 25 | extraArgs.keepLast = false; 26 | extraArgs.execParameters.useCuda = false; 27 | extraArgs.execParameters.num_of_gpus = 0; 28 | extraArgs.execParameters.num_of_threads = 0; 29 | extraArgs.execParameters.delayedDerivMinMax = 30 | levelset::DelayedDerivMinMax_Disable; 31 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = true; 32 | 33 | return extraArgs; 34 | } 35 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Reachability Computation/def_extraArgs.cpp: -------------------------------------------------------------------------------- 1 | helperOC::HJIPDE_extraArgs def_extraArgs( 2 | bool accel, 3 | helperOC::DynSys* p) { 4 | 5 | helperOC::HJIPDE_extraArgs extraArgs; 6 | 7 | // Target set and visualization 8 | extraArgs.visualize = true; 9 | 10 | if (accel) { 11 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0, 0, 0}; 12 | extraArgs.plotData.projpt = 13 | beacls::FloatVec{p->get_x()[2], p->get_x()[3], p->get_x()[4]}; 14 | } 15 | else { 16 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0}; 17 | extraArgs.plotData.projpt = beacls::FloatVec{p->get_x()[2]}; 18 | } 19 | 20 | extraArgs.deleteLastPlot = true; 21 | extraArgs.fig_filename = "figs/Car_test"; 22 | 23 | extraArgs.execParameters.line_length_of_chunk = 1; 24 | extraArgs.execParameters.calcTTR = false; 25 | extraArgs.keepLast = false; 26 | extraArgs.execParameters.useCuda = false; 27 | extraArgs.execParameters.num_of_gpus = 0; 28 | extraArgs.execParameters.num_of_threads = 0; 29 | extraArgs.execParameters.delayedDerivMinMax = 30 | levelset::DelayedDerivMinMax_Disable; 31 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = true; 32 | 33 | return extraArgs; 34 | } 35 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Car4D_Car1D_Intersection/def_extraArgs.cpp: -------------------------------------------------------------------------------- 1 | helperOC::HJIPDE_extraArgs def_extraArgs( 2 | bool accel, 3 | helperOC::DynSys* p) { 4 | 5 | helperOC::HJIPDE_extraArgs extraArgs; 6 | 7 | // Target set and visualization 8 | extraArgs.visualize = true; 9 | 10 | if (accel) { 11 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0, 0, 0}; 12 | extraArgs.plotData.projpt = 13 | beacls::FloatVec{p->get_x()[2], p->get_x()[3], p->get_x()[4]}; 14 | } 15 | else { 16 | extraArgs.plotData.plotDims = beacls::IntegerVec{ 1, 1, 0}; 17 | extraArgs.plotData.projpt = beacls::FloatVec{p->get_x()[2]}; 18 | } 19 | 20 | extraArgs.deleteLastPlot = true; 21 | extraArgs.fig_filename = "figs/Car_test"; 22 | 23 | extraArgs.execParameters.line_length_of_chunk = 1; 24 | extraArgs.execParameters.calcTTR = false; 25 | extraArgs.keepLast = false; 26 | extraArgs.execParameters.useCuda = false; 27 | extraArgs.execParameters.num_of_gpus = 0; 28 | extraArgs.execParameters.num_of_threads = 0; 29 | extraArgs.execParameters.delayedDerivMinMax = 30 | levelset::DelayedDerivMinMax_Disable; 31 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = true; 32 | 33 | return extraArgs; 34 | } 35 | -------------------------------------------------------------------------------- /STL Functions/always.cpp: -------------------------------------------------------------------------------- 1 | int always( 2 | std::vector datas, 3 | beacls::FloatVec alpha, 4 | FLOAT_TYPE tau1, 5 | FLOAT_TYPE tau2, 6 | helperOC::DynSysSchemeData* schemeData, 7 | beacls::FloatVec tau, 8 | helperOC::HJIPDE_extraArgs extraArgs){ 9 | 10 | const FLOAT_TYPE small = 1e-3; 11 | const size_t numel = schemeData->get_grid()->get_numel(); 12 | 13 | std::vector targets(tau.size()); 14 | for (size_t i = 0; i < tau.size(); ++i) { 15 | targets[i].assign(numel, 100.); 16 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 17 | // avoid alpha if tau1 < tau < tau2 18 | std::copy(alpha.begin(), alpha.end(), targets[i].begin()); 19 | } 20 | } 21 | 22 | schemeData->uMode = helperOC::DynSys_UMode_Max; 23 | schemeData->dMode = helperOC::DynSys_DMode_Min; 24 | 25 | helperOC::HJIPDE_extraOuts extraOuts; 26 | 27 | extraArgs.targets = targets; 28 | 29 | helperOC::HJIPDE* hjipde; 30 | hjipde = new helperOC::HJIPDE(); 31 | 32 | beacls::FloatVec tau_out; 33 | 34 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 35 | helperOC::HJIPDE::MinWithType_None, extraArgs); 36 | 37 | if (hjipde) delete hjipde; 38 | return 0; 39 | } -------------------------------------------------------------------------------- /Test Sample/always.cpp: -------------------------------------------------------------------------------- 1 | int always( 2 | std::vector datas, 3 | beacls::FloatVec alpha, 4 | FLOAT_TYPE tau1, 5 | FLOAT_TYPE tau2, 6 | helperOC::DynSysSchemeData* schemeData, 7 | beacls::FloatVec tau, 8 | helperOC::HJIPDE_extraArgs extraArgs){ 9 | 10 | const FLOAT_TYPE small = 1e-3; 11 | const size_t numel = schemeData->get_grid()->get_numel(); 12 | 13 | std::vector targets(tau.size()); 14 | for (size_t i = 0; i < tau.size(); ++i) { 15 | targets[i].assign(numel, 100.); 16 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 17 | // avoid alpha if tau1 < tau < tau2 18 | std::copy(alpha.begin(), alpha.end(), targets[i].begin()); 19 | } 20 | } 21 | 22 | schemeData->uMode = helperOC::DynSys_UMode_Max; 23 | schemeData->dMode = helperOC::DynSys_DMode_Min; 24 | 25 | helperOC::HJIPDE_extraOuts extraOuts; 26 | 27 | extraArgs.targets = targets; 28 | 29 | helperOC::HJIPDE* hjipde; 30 | hjipde = new helperOC::HJIPDE(); 31 | 32 | beacls::FloatVec tau_out; 33 | 34 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 35 | helperOC::HJIPDE::MinWithType_None, extraArgs); 36 | 37 | if (hjipde) delete hjipde; 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/add_pyramid.cpp: -------------------------------------------------------------------------------- 1 | void add_pyramid( 2 | beacls::FloatVec& alpha, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE dim, 5 | const size_t numel, 6 | FLOAT_TYPE alpha_offset, 7 | FLOAT_TYPE length){ 8 | 9 | if (dim == 0){ 10 | //reset the default values 11 | for (int i = 0; i25 28 | t = 25; 29 | end 30 | tstep_sim = 0.25; 31 | i = floor(t/tstep_sim); %which alpha_U_beta to evaluate 32 | 33 | traj = [x,y,th,V,y2]; 34 | 35 | %compute gradient of value function and control action 36 | [u,~,~] = eval_u_deriv(traj,gmatOut,deriv,deriv_R,deriv_L,0,NaN,i+2); 37 | u = [u(2),u(1)]'; 38 | 39 | if isempty(find(isnan(u),1))==0 40 | fprintf("error!") 41 | end 42 | u(isnan(u)) = 0; 43 | msg.Data = double(u); 44 | send(ctrlpub,msg); 45 | 46 | end 47 | 48 | %rosshutdown 49 | 50 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Hardware Experiment/ROS_Overtake.m: -------------------------------------------------------------------------------- 1 | 2 | clear all 3 | global deriv deriv_R deriv_L gmatOut 4 | load deriv.mat 5 | load derivLR.mat 6 | % %rosshutdown 7 | rosinit('http://joe.local:11311'); 8 | %rosinit 9 | global ctrlpub msg 10 | 11 | ctrlpub = rospublisher('/ctrl_MATLAB','std_msgs/Float64MultiArray'); 12 | msg = rosmessage(ctrlpub); 13 | 14 | rossubscriber('/StateSpace',@ctrl_for_joe); 15 | 16 | function ctrl_for_joe(~,message) 17 | global deriv deriv_R deriv_L gmatOut 18 | global ctrlpub msg 19 | x = message.Data(1);%message.x; 20 | y = message.Data(2);%message.y; 21 | %th = wrapTo2Pi(message.Data(3));%message.theta; 22 | th = message.Data(3); 23 | V = message.Data(4);%message.V; 24 | y2 = message.Data(5);%message.y2; 25 | t = message.Data(6); 26 | 27 | if t>25 28 | t = 25; 29 | end 30 | tstep_sim = 0.25; 31 | i = floor(t/tstep_sim); %which alpha_U_beta to evaluate 32 | 33 | traj = [x,y,th,V,y2]; 34 | 35 | %compute gradient of value function and control action 36 | [u,~,~] = eval_u_deriv(traj,gmatOut,deriv,deriv_R,deriv_L,0,NaN,i+2); 37 | u = [u(2),u(1)]'; 38 | 39 | if isempty(find(isnan(u),1))==0 40 | fprintf("error!") 41 | end 42 | u(isnan(u)) = 0; 43 | msg.Data = double(u); 44 | send(ctrlpub,msg); 45 | 46 | end 47 | 48 | %rosshutdown 49 | 50 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/always.cpp: -------------------------------------------------------------------------------- 1 | int always( 2 | std::vector& datas, 3 | beacls::FloatVec alpha, 4 | FLOAT_TYPE tau1, 5 | FLOAT_TYPE tau2, 6 | helperOC::DynSysSchemeData* schemeData, 7 | beacls::FloatVec tau, 8 | helperOC::HJIPDE_extraArgs extraArgs){ 9 | 10 | const FLOAT_TYPE small = 1e-3; 11 | const size_t numel = schemeData->get_grid()->get_numel(); 12 | 13 | std::vector targets(tau.size()); 14 | for (size_t i = 0; i < tau.size(); ++i) { 15 | targets[i].assign(numel, 100.); 16 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 17 | // avoid alpha if tau1 < tau < tau2 18 | std::copy(alpha.begin(), alpha.end(), targets[i].begin()); 19 | } 20 | } 21 | 22 | schemeData->uMode = helperOC::DynSys_UMode_Max; 23 | schemeData->dMode = helperOC::DynSys_DMode_Min; 24 | 25 | helperOC::HJIPDE_extraOuts extraOuts; 26 | 27 | extraArgs.targets = targets; 28 | 29 | helperOC::HJIPDE* hjipde; 30 | hjipde = new helperOC::HJIPDE(); 31 | 32 | beacls::FloatVec tau_out; 33 | 34 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 35 | helperOC::HJIPDE::MinWithType_None, extraArgs); 36 | 37 | if (hjipde) delete hjipde; 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/PID_Controller.m: -------------------------------------------------------------------------------- 1 | function [u,integral] = PID_Controller(th,V,J_th,J_V,dt,th_mat,V_mat,integral) 2 | 3 | w_mat = [-0.4,0.4]; %range of allowable turning rate 4 | a_mat = [-0.2,0.2]; %range of allowable acceleration 5 | 6 | Ki = 1; 7 | Kp = 2.5*Ki; 8 | 9 | Kp_th = Kp; 10 | Kp_V = Kp; 11 | 12 | Ki_th = Ki; 13 | Ki_V = Ki; 14 | 15 | [~,opt_th_I] = max(J_th); 16 | [~,opt_V_I] = max(J_V); 17 | 18 | opt_th = th_mat(opt_th_I); 19 | opt_V = V_mat(opt_V_I); 20 | 21 | error_th = opt_th-th; 22 | error_V = opt_V-V; 23 | 24 | integral_old = integral; 25 | integral(1) = integral(1) + error_th*dt; 26 | integral(2) = integral(2) + error_V*dt; 27 | 28 | u(1) = Kp_th*error_th + Ki_th*integral(1); 29 | u(2) = Kp_V*error_V + Ki_V*integral(2); 30 | 31 | if u(1)>w_mat(2) 32 | u(1) = w_mat(2); 33 | integral(1) = integral_old(1); 34 | elseif u(1)a_mat(2) 40 | u(2) = a_mat(2); 41 | integral(2) = integral_old(2); 42 | elseif u(2)vrange[1]){ 26 | // lane[i] = -10.; 27 | // } 28 | // } 29 | // } 30 | // else if (dim == 4) { 31 | // for (int i = 0; i <= lane.size()-1; ++i) { 32 | // if (xs[i]y2range[1]){ 33 | // lane[i] = -10.; 34 | // } 35 | // } 36 | // } 37 | } 38 | -------------------------------------------------------------------------------- /Old Files/Car4D_Car1D_Intersection/add_lane.cpp: -------------------------------------------------------------------------------- 1 | void add_lane( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec vrange){ 10 | 11 | 12 | if (dim == 0 || dim == 1){ 13 | //reset the default values 14 | std::fill(lane.begin(),lane.end(),0.); 15 | // for (int i = 0; ivrange[1]){ 36 | lane[i] = -5.; 37 | } 38 | } 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/get_subvector.cpp: -------------------------------------------------------------------------------- 1 | void get_subvector( 2 | beacls::FloatVec& alpha_temp, 3 | beacls::FloatVec alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | FLOAT_TYPE x_unit = (shape[0]-1)/(gmax[0]-gmin[0]); 10 | FLOAT_TYPE y_unit = (shape[1]-1)/(gmax[1]-gmin[1]); 11 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 12 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 13 | beacls::IntegerVec x_index{x1,x2}; 14 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 15 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 16 | beacls::IntegerVec y_index{y1,y2}; 17 | 18 | alpha_temp.clear(); 19 | int x_size, y_size, y, z; 20 | beacls::IntegerVec n; 21 | x_size = shape[0]; 22 | y_size = shape[1]; 23 | 24 | unsigned long int n_dist = 0; 25 | 26 | for (z = 0; z <= shape[2]-1; ++z) { 27 | for (y = y_index[0]; y <= y_index[1]; ++y){ 28 | n = {z*y_size*x_size + y*x_size + x_index[0], z*y_size*x_size + y*x_size + x_index[1]}; 29 | alpha_temp.insert(alpha_temp.end(),alpha.begin()+n[0],alpha.begin()+n[1]+1); 30 | 31 | } 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_frame.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_frame( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec vrange, 10 | beacls::FloatVec y2range){ 11 | 12 | 13 | if (dim == 0 || dim == 1){ 14 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1)/(gmax[0]-gmin[0]); 15 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 16 | 17 | long unsigned int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 18 | 19 | FLOAT_TYPE y_add = (y_unit*vehicle_width); 20 | FLOAT_TYPE x_add = (x_unit*vehicle_width*0.78); 21 | 22 | x_size = x2-x1+1; 23 | y_size = y2-y1+1; 24 | z_size = shape[2]; 25 | a_size = shape[3]; 26 | b_size = shape[4]; 27 | 28 | beacls::IntegerVec y_addvec{(long unsigned int)((static_cast(y_size)-1.)/2.-y_add/2.+0.5),(long unsigned int)((static_cast(x_size)-1.)/2.+y_add/2.+0.5)}; 29 | 30 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), 31 | [lane_offset, lane_width, vehicle_width](const auto &xs_i) { 32 | return 2.5*(1. - std::pow(((xs_i - lane_offset)/((lane_width-vehicle_width)/2.)),2)); }); 33 | 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/update_lane.cpp: -------------------------------------------------------------------------------- 1 | void update_lane( 2 | beacls::FloatVec alpha_temp, 3 | beacls::FloatVec& alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (shape[0]-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (shape[1]-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | int x_size, y_size, y, z; 20 | unsigned long int n_dist = 0; 21 | beacls::IntegerVec n; 22 | x_size = shape[0]; 23 | y_size = shape[1]; 24 | 25 | for (z = 0; z <= shape[2]-1; ++z) { 26 | for (y = y_index[0]; y <= y_index[1]; ++y){ 27 | n = {z*y_size*x_size + y*x_size + x_index[0], z*y_size*x_size + y*x_size + x_index[1]}; 28 | std::copy(alpha_temp.begin()+n_dist,alpha_temp.begin()+n_dist+n[1]-n[0]+1,alpha.begin()+n[0]); 29 | n_dist = n_dist + n[1] - n[0] + 1; 30 | 31 | } 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /Old Files/Car4D_Car1D_Intersection/add_barrier.cpp: -------------------------------------------------------------------------------- 1 | 2 | void add_barrier( 3 | beacls::FloatVec& lane, 4 | beacls::FloatVec xs, 5 | FLOAT_TYPE lane_offset, 6 | FLOAT_TYPE lane_width, 7 | FLOAT_TYPE theta_offset, 8 | FLOAT_TYPE vehicle_width, 9 | size_t dim, 10 | int enhance){ 11 | 12 | 13 | if (dim == 0 || dim == 1){ 14 | //reset the default values 15 | for (int i = 0; ivrange[1]){ 37 | lane[i] = -10.; 38 | } 39 | } 40 | } 41 | else if (dim == 4) { 42 | for (int i = 0; i <= lane.size()-1; ++i) { 43 | if (xs[i]y2range[1]){ 44 | lane[i] = -10.; 45 | } 46 | } 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane.cpp: -------------------------------------------------------------------------------- 1 | void add_lane( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec vrange, 10 | beacls::FloatVec y2range){ 11 | 12 | 13 | if (dim == 0 || dim == 1){ 14 | //reset the default values 15 | std::fill(lane.begin(),lane.end(),0.); 16 | // for (int i = 0; ivrange[1]){ 37 | lane[i] = -10.; 38 | } 39 | } 40 | } 41 | else if (dim == 4) { 42 | for (int i = 0; i <= lane.size()-1; ++i) { 43 | if (xs[i]y2range[1]){ 44 | lane[i] = -10.; 45 | } 46 | } 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Car4D_Car1D_Intersection/add_lane.cpp: -------------------------------------------------------------------------------- 1 | void add_lane( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec vrange, 10 | beacls::FloatVec y2range){ 11 | 12 | 13 | if (dim == 0 || dim == 1){ 14 | //reset the default values 15 | std::fill(lane.begin(),lane.end(),0.); 16 | // for (int i = 0; ivrange[1]){ 37 | lane[i] = -10.; 38 | } 39 | } 40 | } 41 | else if (dim == 4) { 42 | for (int i = 0; i <= lane.size()-1; ++i) { 43 | if (xs[i]y2range[1]){ 44 | lane[i] = -10.; 45 | } 46 | } 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Numerical Simulation/ROS_Overtake.m: -------------------------------------------------------------------------------- 1 | clear all 2 | global deriv deriv_R deriv_L gmatOut always_alpha 3 | load deriv.mat 4 | load derivLR.mat 5 | load always_alpha.mat 6 | % %rosshutdown 7 | rosinit('http://joe.local:11311'); 8 | %rosinit 9 | global ctrlpub msg 10 | 11 | ctrlpub = rospublisher('/ctrl_MATLAB','std_msgs/Float64MultiArray'); 12 | msg = rosmessage(ctrlpub); 13 | 14 | rossubscriber('/StateSpace',@ctrl_for_joe); 15 | 16 | function ctrl_for_joe(~,message) 17 | global deriv deriv_R deriv_L gmatOut 18 | global ctrlpub msg 19 | x = message.Data(1);%message.x; 20 | y = message.Data(2);%message.y; 21 | %th = wrapTo2Pi(message.Data(3));%message.theta; 22 | th = message.Data(3); 23 | V = message.Data(4);%message.V; 24 | y2 = message.Data(5);%message.y2; 25 | t = message.Data(6); 26 | 27 | if t>25 28 | t = 25; 29 | end 30 | tstep_sim = 0.25; 31 | i = floor(t/tstep_sim); %which alpha_U_beta to evaluate 32 | 33 | traj = [x,y,th,V,y2]; 34 | 35 | [~,value_always] = eval_u(traj,gmatOut,always_alpha{end+2-i}(:,:,:,:,:),... 36 | always_alpha{end+2-i}(:,:,:,:,:),tstep_sim,0); 37 | 38 | if value_always >= 0 39 | %compute gradient of value function and control action 40 | [u,~,~] = eval_u_deriv(traj,gmatOut,deriv,deriv_R,deriv_L,0,NaN,i+2); 41 | else 42 | %LFRCS 43 | [u,~,~] = eval_u_deriv_halfplane(traj,gmatOut,deriv,deriv_R,deriv_L,tstep_sim,0,i+2,intercept_Coefficient,slope_Coefficient); 44 | end 45 | 46 | u = [u(2),u(1)]'; 47 | 48 | if isempty(find(isnan(u),1))==0 49 | fprintf("error!") 50 | end 51 | u(isnan(u)) = 0; 52 | msg.Data = double(u); 53 | send(ctrlpub,msg); 54 | 55 | end 56 | 57 | %rosshutdown 58 | 59 | -------------------------------------------------------------------------------- /Old Files/Car4D_Car1D_Intersection/update_lane.cpp: -------------------------------------------------------------------------------- 1 | void update_lane( 2 | beacls::FloatVec alpha_temp, 3 | beacls::FloatVec& alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (shape[0]-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (shape[1]-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 20 | unsigned long int n_dist = 0; 21 | beacls::IntegerVec n; 22 | x_size = shape[0]; 23 | y_size = shape[1]; 24 | z_size = shape[2]; 25 | a_size = shape[3]; 26 | b_size = shape[4]; 27 | 28 | for (b = 0; b <= b_size-1; ++b) { 29 | for (a = 0; a <= a_size-1; ++a) { 30 | for (z = 0; z <= z_size-1; ++z) { 31 | for (y = y_index[0]; y <= y_index[1]; ++y){ 32 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[1]}; 33 | std::copy(alpha_temp.begin()+n_dist,alpha_temp.begin()+n_dist+n[1]-n[0]+1,alpha.begin()+n[0]); 34 | n_dist = n_dist + n[1] - n[0] + 1; 35 | } 36 | } 37 | } 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /Old Files/Car4D_Car1D_Intersection/get_subvector.cpp: -------------------------------------------------------------------------------- 1 | void get_subvector( 2 | beacls::FloatVec& alpha_temp, 3 | beacls::FloatVec alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1.)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | alpha_temp.clear(); 20 | int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 21 | unsigned long int n_dist = 0; 22 | beacls::IntegerVec n; 23 | x_size = shape[0]; 24 | y_size = shape[1]; 25 | z_size = shape[2]; 26 | a_size = shape[3]; 27 | b_size = shape[4]; 28 | 29 | for (b = 0; b <= b_size-1; ++b) { 30 | for (a = 0; a <= a_size-1; ++a) { 31 | for (z = 0; z <= z_size-1; ++z) { 32 | for (y = y_index[0]; y <= y_index[1]; ++y){ 33 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[1]}; 34 | alpha_temp.insert(alpha_temp.end(),alpha.begin()+n[0],alpha.begin()+n[1]+1); 35 | } 36 | } 37 | } 38 | } 39 | 40 | } 41 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/computeGradient_C.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | void ComputeGradient_C( 5 | std::vector& derivC, 6 | std::vector& derivL, 7 | std::vector& derivR, 8 | levelset::HJI_Grid* g, 9 | std::vector& data) 10 | { 11 | 12 | const bool calculateTTRduringSolving = false; 13 | bool useCuda = false; 14 | int num_of_threads = 0; 15 | int num_of_gpus = 0; 16 | size_t line_length_of_chunk = 1; 17 | bool enable_user_defined_dynamics_on_gpu = true; 18 | 19 | helperOC::HJIPDE_extraArgs extraArgs; 20 | levelset::DelayedDerivMinMax_Type delayedDerivMinMax = levelset::DelayedDerivMinMax_Disable; 21 | extraArgs.execParameters.line_length_of_chunk = line_length_of_chunk; 22 | extraArgs.execParameters.calcTTR = calculateTTRduringSolving; 23 | extraArgs.execParameters.useCuda = useCuda; 24 | extraArgs.execParameters.num_of_gpus = num_of_gpus; 25 | extraArgs.execParameters.num_of_threads = num_of_threads; 26 | extraArgs.execParameters.delayedDerivMinMax = delayedDerivMinMax; 27 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = 28 | enable_user_defined_dynamics_on_gpu; 29 | const helperOC::ExecParameters execParameters = extraArgs.execParameters; 30 | const beacls::UVecType execType = (execParameters.useCuda) ? beacls::UVecType_Cuda : beacls::UVecType_Vector; 31 | //bool computeGradients = new ComputeGradients(g, helperOC::ApproximationAccuracy_veryHigh, execType); 32 | ComputeGradients* computeGradients; 33 | computeGradients->operator()(derivC, derivL, derivR, g, data, data.size(), false, execParameters); 34 | } 35 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/update_lane.cpp: -------------------------------------------------------------------------------- 1 | void update_lane( 2 | beacls::FloatVec alpha_temp, 3 | beacls::FloatVec& alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (shape[0]-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (shape[1]-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 20 | unsigned long int n_dist = 0; 21 | beacls::IntegerVec n; 22 | x_size = shape[0]; 23 | y_size = shape[1]; 24 | z_size = shape[2]; 25 | a_size = shape[3]; 26 | b_size = shape[4]; 27 | 28 | for (b = 0; b <= b_size-1; ++b) { 29 | for (a = 0; a <= a_size-1; ++a) { 30 | for (z = 0; z <= z_size-1; ++z) { 31 | for (y = y_index[0]; y <= y_index[1]; ++y){ 32 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[1]}; 33 | std::copy(alpha_temp.begin()+n_dist,alpha_temp.begin()+n_dist+n[1]-n[0]+1,alpha.begin()+n[0]); 34 | n_dist = n_dist + n[1] - n[0] + 1; 35 | } 36 | } 37 | } 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Reachability Computation/update_lane.cpp: -------------------------------------------------------------------------------- 1 | void update_lane( 2 | beacls::FloatVec alpha_temp, 3 | beacls::FloatVec& alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (shape[0]-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (shape[1]-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 20 | unsigned long int n_dist = 0; 21 | beacls::IntegerVec n; 22 | x_size = shape[0]; 23 | y_size = shape[1]; 24 | z_size = shape[2]; 25 | a_size = shape[3]; 26 | b_size = shape[4]; 27 | 28 | for (b = 0; b <= b_size-1; ++b) { 29 | for (a = 0; a <= a_size-1; ++a) { 30 | for (z = 0; z <= z_size-1; ++z) { 31 | for (y = y_index[0]; y <= y_index[1]; ++y){ 32 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[1]}; 33 | std::copy(alpha_temp.begin()+n_dist,alpha_temp.begin()+n_dist+n[1]-n[0]+1,alpha.begin()+n[0]); 34 | n_dist = n_dist + n[1] - n[0] + 1; 35 | } 36 | } 37 | } 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/get_subvector.cpp: -------------------------------------------------------------------------------- 1 | void get_subvector( 2 | beacls::FloatVec& alpha_temp, 3 | beacls::FloatVec alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1.)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | alpha_temp.clear(); 20 | int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 21 | unsigned long int n_dist = 0; 22 | beacls::IntegerVec n; 23 | x_size = shape[0]; 24 | y_size = shape[1]; 25 | z_size = shape[2]; 26 | a_size = shape[3]; 27 | b_size = shape[4]; 28 | 29 | for (b = 0; b <= b_size-1; ++b) { 30 | for (a = 0; a <= a_size-1; ++a) { 31 | for (z = 0; z <= z_size-1; ++z) { 32 | for (y = y_index[0]; y <= y_index[1]; ++y){ 33 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[1]}; 34 | alpha_temp.insert(alpha_temp.end(),alpha.begin()+n[0],alpha.begin()+n[1]+1); 35 | } 36 | } 37 | } 38 | } 39 | 40 | } 41 | -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/eval_u_deriv.m: -------------------------------------------------------------------------------- 1 | function [u,Vq_V,Vq_th] = eval_u_deriv(state,g,deriv_th,deriv_V,deriv_V_R,deriv_V_L,t_step,value) 2 | u = zeros(2,1); 3 | 4 | vrange = [0,0.15]; %range of acceptable velocities 5 | w_mat = [-0.4,0.4]; %range of allowable turning rate 6 | a_mat = [-0.2,0.2]; %range of allowable acceleration 7 | v_mat = linspace(g.min(4),g.max(4),g.N(4)); 8 | v_thresh_min = min(v_mat(v_mat>vrange(1))); 9 | v_thresh_max = max(v_mat(v_mat=vrange(1) && Vq<=v_thresh_min 24 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V_R,xq,yq,thq,Vq,y2q); 25 | elseif Vq<=vrange(2) && Vq>=v_thresh_max 26 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V_L,xq,yq,thq,Vq,y2q); 27 | else 28 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V,xq,yq,thq,Vq,y2q); 29 | end 30 | 31 | if Vq_th==0 32 | u(1) = 0; 33 | elseif Vq_th<0 34 | u(1) = min(w_mat); 35 | elseif Vq_th>0 36 | u(1) = max(w_mat); 37 | end 38 | 39 | if Vq_V==0 40 | u(2) = 0; 41 | elseif Vq_V<0 42 | u(2) = min(a_mat); 43 | elseif Vq_V>=0 44 | u(2) = max(a_mat); 45 | end 46 | 47 | % if value <= val_threshold 48 | % u(1:2) = 0; 49 | % end 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Reachability Computation/get_subvector.cpp: -------------------------------------------------------------------------------- 1 | void get_subvector( 2 | beacls::FloatVec& alpha_temp, 3 | beacls::FloatVec alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1.)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | alpha_temp.clear(); 20 | int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 21 | unsigned long int n_dist = 0; 22 | beacls::IntegerVec n; 23 | x_size = shape[0]; 24 | y_size = shape[1]; 25 | z_size = shape[2]; 26 | a_size = shape[3]; 27 | b_size = shape[4]; 28 | 29 | for (b = 0; b <= b_size-1; ++b) { 30 | for (a = 0; a <= a_size-1; ++a) { 31 | for (z = 0; z <= z_size-1; ++z) { 32 | for (y = y_index[0]; y <= y_index[1]; ++y){ 33 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[1]}; 34 | alpha_temp.insert(alpha_temp.end(),alpha.begin()+n[0],alpha.begin()+n[1]+1); 35 | } 36 | } 37 | } 38 | } 39 | 40 | } 41 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Car4D_Car1D_Intersection/update_lane.cpp: -------------------------------------------------------------------------------- 1 | void update_lane( 2 | beacls::FloatVec alpha_temp, 3 | beacls::FloatVec& alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (shape[0]-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (shape[1]-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 20 | unsigned long int n_dist = 0; 21 | beacls::IntegerVec n; 22 | x_size = shape[0]; 23 | y_size = shape[1]; 24 | z_size = shape[2]; 25 | a_size = shape[3]; 26 | b_size = shape[4]; 27 | 28 | for (b = 0; b <= b_size-1; ++b) { 29 | for (a = 0; a <= a_size-1; ++a) { 30 | for (z = 0; z <= z_size-1; ++z) { 31 | for (y = y_index[0]; y <= y_index[1]; ++y){ 32 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[1]}; 33 | std::copy(alpha_temp.begin()+n_dist,alpha_temp.begin()+n_dist+n[1]-n[0]+1,alpha.begin()+n[0]); 34 | n_dist = n_dist + n[1] - n[0] + 1; 35 | } 36 | } 37 | } 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/eval_u_deriv.m: -------------------------------------------------------------------------------- 1 | function [u,Vq_V,Vq_th] = eval_u_deriv(state,g,deriv_th,deriv_V,deriv_V_R,deriv_V_L,t_step,value) 2 | u = zeros(2,1); 3 | 4 | vrange = [0,0.15]; %range of acceptable velocities 5 | w_mat = [-0.4,0.4]; %range of allowable turning rate 6 | a_mat = [-0.2,0.2]; %range of allowable acceleration 7 | v_mat = linspace(g.min(4),g.max(4),g.N(4)); 8 | v_thresh_min = min(v_mat(v_mat>vrange(1))); 9 | v_thresh_max = max(v_mat(v_mat=vrange(1) && Vq<=v_thresh_min 24 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V_R,xq,yq,thq,Vq,y2q); 25 | elseif Vq<=vrange(2) && Vq>=v_thresh_max 26 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V_L,xq,yq,thq,Vq,y2q); 27 | else 28 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V,xq,yq,thq,Vq,y2q); 29 | end 30 | 31 | if Vq_th==0 32 | u(1) = 0; 33 | elseif Vq_th<0 34 | u(1) = min(w_mat); 35 | elseif Vq_th>0 36 | u(1) = max(w_mat); 37 | end 38 | 39 | if Vq_V==0 40 | u(2) = 0; 41 | elseif Vq_V<0 42 | u(2) = min(a_mat); 43 | elseif Vq_V>=0 44 | u(2) = max(a_mat); 45 | end 46 | 47 | % if value <= val_threshold 48 | % u(1:2) = 0; 49 | % end 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /Test Sample/until.cpp: -------------------------------------------------------------------------------- 1 | int until( 2 | std::vector& datas, 3 | beacls::FloatVec alpha, 4 | beacls::FloatVec beta, 5 | FLOAT_TYPE tau1, 6 | FLOAT_TYPE tau2, 7 | helperOC::DynSysSchemeData* schemeData, 8 | beacls::FloatVec tau, 9 | helperOC::HJIPDE_extraArgs extraArgs) { 10 | 11 | const FLOAT_TYPE small = 1e-3; 12 | const size_t numel = schemeData->get_grid()->get_numel(); 13 | 14 | std::vector targets(tau.size()); 15 | for (size_t i = 0; i < tau.size(); ++i) { 16 | targets[i].assign(numel, 100.); 17 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 18 | // satisfy beta if tau1 < tau < tau2, but also negate 19 | std::transform(beta.cbegin(), beta.cend(), targets[i].begin(), 20 | std::negate()); 21 | } 22 | } 23 | 24 | std::vector obstacles(1); 25 | obstacles[0].assign(numel, 0.); 26 | std::copy(alpha.cbegin(), alpha.cend(), obstacles[0].begin()); 27 | 28 | 29 | schemeData->uMode = helperOC::DynSys_UMode_Min; 30 | schemeData->dMode = helperOC::DynSys_DMode_Max; 31 | 32 | helperOC::HJIPDE_extraOuts extraOuts; 33 | 34 | extraArgs.targets = targets; 35 | extraArgs.obstacles = obstacles; 36 | 37 | helperOC::HJIPDE* hjipde; 38 | hjipde = new helperOC::HJIPDE(); 39 | 40 | beacls::FloatVec tau_out; 41 | 42 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 43 | helperOC::HJIPDE::MinWithType_None, extraArgs); 44 | 45 | // Negate result to follow STL convention 46 | for (size_t i = 0; i < tau.size(); ++i) { 47 | std::transform(datas[i].cbegin(), datas[i].cend(), datas[i].begin(), 48 | std::negate()); 49 | } 50 | 51 | if (hjipde) delete hjipde; 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Car4D_Car1D_Intersection/get_subvector.cpp: -------------------------------------------------------------------------------- 1 | void get_subvector( 2 | beacls::FloatVec& alpha_temp, 3 | beacls::FloatVec alpha, 4 | const std::vector shape, 5 | beacls::FloatVec range, 6 | beacls::FloatVec gmin, 7 | beacls::FloatVec gmax, 8 | const size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1.)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | alpha_temp.clear(); 20 | int x_size, y_size, z_size, a_size, b_size, y, z, a, b; 21 | unsigned long int n_dist = 0; 22 | beacls::IntegerVec n; 23 | x_size = shape[0]; 24 | y_size = shape[1]; 25 | z_size = shape[2]; 26 | a_size = shape[3]; 27 | b_size = shape[4]; 28 | 29 | for (b = 0; b <= b_size-1; ++b) { 30 | for (a = 0; a <= a_size-1; ++a) { 31 | for (z = 0; z <= z_size-1; ++z) { 32 | for (y = y_index[0]; y <= y_index[1]; ++y){ 33 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_index[1]}; 34 | alpha_temp.insert(alpha_temp.end(),alpha.begin()+n[0],alpha.begin()+n[1]+1); 35 | } 36 | } 37 | } 38 | } 39 | 40 | } 41 | -------------------------------------------------------------------------------- /STL Functions/until.cpp: -------------------------------------------------------------------------------- 1 | int until( 2 | std::vector& datas, 3 | beacls::FloatVec alpha, 4 | beacls::FloatVec beta, 5 | FLOAT_TYPE tau1, 6 | FLOAT_TYPE tau2, 7 | helperOC::DynSysSchemeData* schemeData, 8 | beacls::FloatVec tau, 9 | helperOC::HJIPDE_extraArgs extraArgs) { 10 | 11 | const FLOAT_TYPE small = 1e-3; 12 | const size_t numel = schemeData->get_grid()->get_numel(); 13 | 14 | std::vector targets(tau.size()); 15 | for (size_t i = 0; i < tau.size(); ++i) { 16 | targets[i].assign(numel, 100.); 17 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 18 | // satisfy beta if tau1 < tau < tau2, but also negate 19 | std::transform(beta.cbegin(), beta.cend(), targets[i].begin(), 20 | std::negate()); 21 | } 22 | } 23 | 24 | std::vector obstacles(1); 25 | obstacles[0].assign(numel, 0.); 26 | std::copy(alpha.cbegin(), alpha.cend(), obstacles[0].begin()); 27 | 28 | 29 | schemeData->uMode = helperOC::DynSys_UMode_Min; 30 | schemeData->dMode = helperOC::DynSys_DMode_Max; 31 | 32 | helperOC::HJIPDE_extraOuts extraOuts; 33 | 34 | extraArgs.targets = targets; 35 | extraArgs.obstacles = obstacles; 36 | 37 | helperOC::HJIPDE* hjipde; 38 | hjipde = new helperOC::HJIPDE(); 39 | 40 | beacls::FloatVec tau_out; 41 | 42 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 43 | helperOC::HJIPDE::MinWithType_None, extraArgs); 44 | 45 | // Negate result to follow STL convention 46 | for (size_t i = 0; i < tau.size(); ++i) { 47 | std::transform(datas[i].cbegin(), datas[i].cend(), datas[i].begin(), 48 | std::negate()); 49 | } 50 | 51 | if (hjipde) delete hjipde; 52 | return 0; 53 | } -------------------------------------------------------------------------------- /Old Files/Car4D_Car1D_Intersection/until.cpp: -------------------------------------------------------------------------------- 1 | int until( 2 | std::vector& datas, 3 | beacls::FloatVec alpha, 4 | beacls::FloatVec beta, 5 | FLOAT_TYPE tau1, 6 | FLOAT_TYPE tau2, 7 | helperOC::DynSysSchemeData* schemeData, 8 | beacls::FloatVec tau, 9 | helperOC::HJIPDE_extraArgs extraArgs) { 10 | 11 | const FLOAT_TYPE small = 1e-3; 12 | const size_t numel = schemeData->get_grid()->get_numel(); 13 | 14 | std::vector targets(tau.size()); 15 | for (size_t i = 0; i < tau.size(); ++i) { 16 | targets[i].assign(numel, 100.); 17 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 18 | // satisfy beta if tau1 < tau < tau2, but also negate 19 | std::transform(beta.cbegin(), beta.cend(), targets[i].begin(), 20 | std::negate()); 21 | } 22 | } 23 | 24 | std::vector obstacles(1); 25 | obstacles[0].assign(numel, 0.); 26 | std::copy(alpha.cbegin(), alpha.cend(), obstacles[0].begin()); 27 | 28 | 29 | schemeData->uMode = helperOC::DynSys_UMode_Min; 30 | schemeData->dMode = helperOC::DynSys_DMode_Max; 31 | 32 | helperOC::HJIPDE_extraOuts extraOuts; 33 | 34 | extraArgs.targets = targets; 35 | extraArgs.obstacles = obstacles; 36 | 37 | helperOC::HJIPDE* hjipde; 38 | hjipde = new helperOC::HJIPDE(); 39 | 40 | beacls::FloatVec tau_out; 41 | 42 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 43 | helperOC::HJIPDE::MinWithType_None, extraArgs); 44 | 45 | // Negate result to follow STL convention 46 | for (size_t i = 0; i < tau.size(); ++i) { 47 | std::transform(datas[i].cbegin(), datas[i].cend(), datas[i].begin(), 48 | std::negate()); 49 | } 50 | 51 | if (hjipde) delete hjipde; 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/until.cpp: -------------------------------------------------------------------------------- 1 | int until( 2 | std::vector& datas, 3 | beacls::FloatVec alpha, 4 | beacls::FloatVec beta, 5 | FLOAT_TYPE tau1, 6 | FLOAT_TYPE tau2, 7 | helperOC::DynSysSchemeData* schemeData, 8 | beacls::FloatVec tau, 9 | helperOC::HJIPDE_extraArgs extraArgs) { 10 | 11 | const FLOAT_TYPE small = 1e-3; 12 | const size_t numel = schemeData->get_grid()->get_numel(); 13 | 14 | std::vector targets(tau.size()); 15 | for (size_t i = 0; i < tau.size(); ++i) { 16 | targets[i].assign(numel, 100.); 17 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 18 | // satisfy beta if tau1 < tau < tau2, but also negate 19 | std::transform(beta.cbegin(), beta.cend(), targets[i].begin(), 20 | std::negate()); 21 | } 22 | } 23 | 24 | std::vector obstacles(1); 25 | obstacles[0].assign(numel, 0.); 26 | std::copy(alpha.cbegin(), alpha.cend(), obstacles[0].begin()); 27 | 28 | 29 | schemeData->uMode = helperOC::DynSys_UMode_Min; 30 | schemeData->dMode = helperOC::DynSys_DMode_Max; 31 | 32 | helperOC::HJIPDE_extraOuts extraOuts; 33 | 34 | extraArgs.targets = targets; 35 | extraArgs.obstacles = obstacles; 36 | 37 | helperOC::HJIPDE* hjipde; 38 | hjipde = new helperOC::HJIPDE(); 39 | 40 | beacls::FloatVec tau_out; 41 | 42 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 43 | helperOC::HJIPDE::MinWithType_None, extraArgs); 44 | 45 | // Negate result to follow STL convention 46 | for (size_t i = 0; i < tau.size(); ++i) { 47 | std::transform(datas[i].cbegin(), datas[i].cend(), datas[i].begin(), 48 | std::negate()); 49 | } 50 | 51 | if (hjipde) delete hjipde; 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/eval_u.m: -------------------------------------------------------------------------------- 1 | function [u,Vq_V,Vq_th] = eval_u(state,g,deriv_th,deriv_V,t_step,value) 2 | u = zeros(2,1); 3 | val_threshold = 0; 4 | w_mat = [-0.4,0.4]; %range of allowable turning rate 5 | a_mat = [-0.2,0.2]; %range of allowable acceleration 6 | th_eps = 0;%0.02; %deviation of values from 0 that do not warrant a change in steering angle (u=0) 7 | V_eps = 0; 8 | 9 | %Linear interpolation 10 | 11 | [xd,yd,thd,Vd,y2d] = ndgrid(linspace(g.min(1),g.max(1),g.N(1)),linspace(g.min(2),g.max(2),g.N(2)),... 12 | linspace(g.min(3),g.max(3),g.N(3)),linspace(g.min(4),g.max(4),g.N(4)),linspace(g.min(5),g.max(5),g.N(5))); 13 | 14 | [xq,yq,thq,Vq,y2q] = ndgrid(state(1),state(2),state(3),state(4),state(5)); 15 | 16 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th,xq,yq,thq,Vq,y2q); 17 | 18 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V,xq,yq,thq,Vq,y2q); 19 | 20 | if Vq_th>=-th_eps && Vq_th<=th_eps 21 | u(1) = 0; 22 | elseif Vq_th<=0 23 | if state(3)+min(w_mat)*t_step <= g.min(3) %|| value <= val_threshold 24 | u(1) = 0; 25 | else 26 | u(1) = min(w_mat); 27 | end 28 | elseif Vq_th>=0 29 | if state(3)+max(w_mat)*t_step >= g.max(3) %|| value <= val_threshold 30 | u(1) = 0; 31 | else 32 | u(1) = max(w_mat); 33 | end 34 | end 35 | 36 | if Vq_V>=-V_eps && Vq_V<=V_eps 37 | u(2) = 0; 38 | elseif Vq_V<=0 39 | if state(4)+min(a_mat)*t_step <= g.min(4) 40 | u(2) = 0; 41 | else 42 | u(2) = min(a_mat); 43 | end 44 | elseif Vq_V>=0 45 | if state(4)+max(a_mat)*t_step >= g.max(4) %|| %value <= val_threshold 46 | u(2) = 0; 47 | else 48 | u(2) = max(a_mat); 49 | end 50 | end 51 | 52 | % if value <= val_threshold 53 | % u(1:2) = 0; 54 | % end 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Simulation/eval_u.m: -------------------------------------------------------------------------------- 1 | function u = eval_u(state,g,deriv_t) 2 | w_mat = -1:1; %range of allowable control 3 | eps = 0;%0.02; %deviation of values from 0 that do not warrant a change in steering angle (u=0) 4 | 5 | Ix = 1 + (state(1)-g.min(1))/g.dx(1); 6 | Iy = 1 + (state(2)-g.min(2))/g.dx(2); 7 | 8 | state(3) = wrapTo2Pi(state(3)); 9 | Ith = 1 + (state(3)-g.min(3))/g.dx(3); 10 | 11 | Ix_L = floor(Ix); 12 | Ix_R = ceil(Ix); 13 | Iy_L = floor(Iy); 14 | Iy_R = ceil(Iy); 15 | 16 | Ith_L = floor(Ith); 17 | Ith_R = ceil(Ith); 18 | 19 | if Ith_R > g.N(3) 20 | Ith_R = 1; 21 | end 22 | 23 | if Ith_L > g.N(3) 24 | Ith_L = 1; 25 | end 26 | 27 | if Ix_L == Ix_R 28 | Ixd = 0; 29 | else 30 | Ixd = (Ix-Ix_L)/(Ix_R-Ix_L); 31 | end 32 | 33 | if Iy_L == Iy_R 34 | Iyd = 0; 35 | else 36 | Iyd = (Iy-Iy_L)/(Iy_R-Iy_L); 37 | end 38 | 39 | if Ith_R - Ith_L < 0 40 | Ithd = (Ith-Ith_L)/((Ith_R+g.max(3))-Ith_L); 41 | elseif Ith_L == Ith_R 42 | Ithd = 0; 43 | else 44 | Ithd = (Ith-Ith_L)/(Ith_R-Ith_L); 45 | end 46 | 47 | %Trilinear interpolation 48 | deriv_th = deriv_t{3}; 49 | c000 = deriv_th(Ix_L,Iy_L,Ith_L); 50 | c100 = deriv_th(Ix_R,Iy_L,Ith_L); 51 | c001 = deriv_th(Ix_L,Iy_L,Ith_R); 52 | c101 = deriv_th(Ix_R,Iy_L,Ith_R); 53 | c010 = deriv_th(Ix_L,Iy_R,Ith_L); 54 | c110 = deriv_th(Ix_R,Iy_R,Ith_L); 55 | c011 = deriv_th(Ix_L,Iy_R,Ith_R); 56 | c111 = deriv_th(Ix_R,Iy_R,Ith_R); 57 | 58 | c00 = c000*(1-Ixd) + c100*Ixd; 59 | c01 = c001*(1-Ixd) + c101*Ixd; 60 | c10 = c010*(1-Ixd) + c110*Ixd; 61 | c11 = c011*(1-Ixd) + c111*Ixd; 62 | 63 | c0 = c00*(1-Iyd)+c10*Iyd; 64 | c1 = c01*(1-Iyd)+c11*Iyd; 65 | 66 | c = c0*(1-Ithd)+c1*Ithd; 67 | 68 | if c>=-eps && c<=eps 69 | u = 0; 70 | elseif c<0 71 | u = min(w_mat); 72 | elseif c>0 73 | u = max(w_mat); 74 | end 75 | 76 | 77 | 78 | 79 | 80 | z -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Reachability Computation/until.cpp: -------------------------------------------------------------------------------- 1 | int until( 2 | std::vector& datas, 3 | beacls::FloatVec alpha, 4 | beacls::FloatVec beta, 5 | FLOAT_TYPE tau1, 6 | FLOAT_TYPE tau2, 7 | helperOC::DynSysSchemeData* schemeData, 8 | beacls::FloatVec tau, 9 | helperOC::HJIPDE_extraArgs extraArgs) { 10 | 11 | const FLOAT_TYPE small = 1e-3; 12 | const size_t numel = schemeData->get_grid()->get_numel(); 13 | 14 | std::vector targets(tau.size()); 15 | for (size_t i = 0; i < tau.size(); ++i) { 16 | targets[i].assign(numel, 100.); 17 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 18 | // satisfy beta if tau1 < tau < tau2, but also negate 19 | std::transform(beta.cbegin(), beta.cend(), targets[i].begin(), 20 | std::negate()); 21 | } 22 | } 23 | 24 | std::vector obstacles(1); 25 | obstacles[0].assign(numel, 0.); 26 | std::copy(alpha.cbegin(), alpha.cend(), obstacles[0].begin()); 27 | 28 | 29 | schemeData->uMode = helperOC::DynSys_UMode_Min; 30 | schemeData->dMode = helperOC::DynSys_DMode_Max; 31 | 32 | helperOC::HJIPDE_extraOuts extraOuts; 33 | 34 | extraArgs.targets = targets; 35 | extraArgs.obstacles = obstacles; 36 | 37 | helperOC::HJIPDE* hjipde; 38 | hjipde = new helperOC::HJIPDE(); 39 | 40 | beacls::FloatVec tau_out; 41 | 42 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 43 | helperOC::HJIPDE::MinWithType_None, extraArgs); 44 | 45 | // Negate result to follow STL convention 46 | for (size_t i = 0; i < tau.size(); ++i) { 47 | std::transform(datas[i].cbegin(), datas[i].cend(), datas[i].begin(), 48 | std::negate()); 49 | } 50 | 51 | if (hjipde) delete hjipde; 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Numerical Simulation/eval_u.m: -------------------------------------------------------------------------------- 1 | function [u,Vq_V,Vq_th] = eval_u(state,g,deriv_th,deriv_V,t_step,value) 2 | u = zeros(2,1); 3 | val_threshold = 0; 4 | w_mat = [-0.4,0.4]; %range of allowable turning rate 5 | a_mat = [-0.2,0.2]; %range of allowable acceleration 6 | th_eps = 0;%0.02; %deviation of values from 0 that do not warrant a change in steering angle (u=0) 7 | V_eps = 0; 8 | 9 | %Linear interpolation 10 | 11 | [xd,yd,thd,Vd,y2d] = ndgrid(linspace(g.min(1),g.max(1),g.N(1)),linspace(g.min(2),g.max(2),g.N(2)),... 12 | linspace(g.min(3),g.max(3),g.N(3)),linspace(g.min(4),g.max(4),g.N(4)),linspace(g.min(5),g.max(5),g.N(5))); 13 | 14 | [xq,yq,thq,Vq,y2q] = ndgrid(state(1),state(2),state(3),state(4),state(5)); 15 | 16 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th,xq,yq,thq,Vq,y2q); 17 | 18 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V,xq,yq,thq,Vq,y2q); 19 | 20 | if Vq_th>=-th_eps && Vq_th<=th_eps 21 | u(1) = 0; 22 | elseif Vq_th<=0 23 | if state(3)+min(w_mat)*t_step <= g.min(3) %|| value <= val_threshold 24 | u(1) = 0; 25 | else 26 | u(1) = min(w_mat); 27 | end 28 | elseif Vq_th>=0 29 | if state(3)+max(w_mat)*t_step >= g.max(3) %|| value <= val_threshold 30 | u(1) = 0; 31 | else 32 | u(1) = max(w_mat); 33 | end 34 | end 35 | 36 | if Vq_V>=-V_eps && Vq_V<=V_eps 37 | u(2) = 0; 38 | elseif Vq_V<=0 39 | if state(4)+min(a_mat)*t_step <= g.min(4) 40 | u(2) = 0; 41 | else 42 | u(2) = min(a_mat); 43 | end 44 | elseif Vq_V>=0 45 | if state(4)+max(a_mat)*t_step >= g.max(4) %|| %value <= val_threshold 46 | u(2) = 0; 47 | else 48 | u(2) = max(a_mat); 49 | end 50 | end 51 | 52 | % if value <= val_threshold 53 | % u(1:2) = 0; 54 | % end 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/eval_u.m: -------------------------------------------------------------------------------- 1 | function [u,Vq_V,Vq_th] = eval_u(state,g,deriv_th,deriv_V,t_step,value) 2 | u = zeros(2,1); 3 | val_threshold = 0; 4 | w_mat = [-0.4,0.4]; %range of allowable turning rate 5 | a_mat = [-0.2,0.2]; %range of allowable acceleration 6 | th_eps = 0;%0.02; %deviation of values from 0 that do not warrant a change in steering angle (u=0) 7 | V_eps = 0; 8 | 9 | %Linear interpolation 10 | 11 | [xd,yd,thd,Vd,y2d] = ndgrid(linspace(g.min(1),g.max(1),g.N(1)),linspace(g.min(2),g.max(2),g.N(2)),... 12 | linspace(g.min(3),g.max(3),g.N(3)),linspace(g.min(4),g.max(4),g.N(4)),linspace(g.min(5),g.max(5),g.N(5))); 13 | 14 | [xq,yq,thq,Vq,y2q] = ndgrid(state(1),state(2),state(3),state(4),state(5)); 15 | 16 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th,xq,yq,thq,Vq,y2q); 17 | 18 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V,xq,yq,thq,Vq,y2q); 19 | 20 | if Vq_th>=-th_eps && Vq_th<=th_eps 21 | u(1) = 0; 22 | elseif Vq_th<=0 23 | if state(3)+min(w_mat)*t_step <= g.min(3) %|| value <= val_threshold 24 | u(1) = 0; 25 | else 26 | u(1) = min(w_mat); 27 | end 28 | elseif Vq_th>=0 29 | if state(3)+max(w_mat)*t_step >= g.max(3) %|| value <= val_threshold 30 | u(1) = 0; 31 | else 32 | u(1) = max(w_mat); 33 | end 34 | end 35 | 36 | if Vq_V>=-V_eps && Vq_V<=V_eps 37 | u(2) = 0; 38 | elseif Vq_V<=0 39 | if state(4)+min(a_mat)*t_step <= g.min(4) 40 | u(2) = 0; 41 | else 42 | u(2) = min(a_mat); 43 | end 44 | elseif Vq_V>=0 45 | if state(4)+max(a_mat)*t_step >= g.max(4) %|| %value <= val_threshold 46 | u(2) = 0; 47 | else 48 | u(2) = max(a_mat); 49 | end 50 | end 51 | 52 | % if value <= val_threshold 53 | % u(1:2) = 0; 54 | % end 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/until.cpp: -------------------------------------------------------------------------------- 1 | int until( 2 | std::vector& datas, 3 | beacls::FloatVec alpha, 4 | beacls::FloatVec beta, 5 | FLOAT_TYPE tau1, 6 | FLOAT_TYPE tau2, 7 | helperOC::DynSysSchemeData* schemeData, 8 | beacls::FloatVec tau, 9 | helperOC::HJIPDE_extraArgs extraArgs) { 10 | 11 | const FLOAT_TYPE small = 1e-3; 12 | const size_t numel = schemeData->get_grid()->get_numel(); 13 | 14 | std::vector targets(tau.size()); 15 | for (size_t i = 0; i < tau.size(); ++i) { 16 | targets[i].assign(numel, 100.); 17 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 18 | // satisfy beta if tau1 < tau < tau2, but also negate 19 | std::transform(beta.cbegin(), beta.cend(), targets[i].begin(), 20 | std::negate()); 21 | } 22 | } 23 | 24 | std::vector obstacles(1); 25 | obstacles[0].assign(numel, 0.); 26 | std::copy(alpha.cbegin(), alpha.cend(), obstacles[0].begin()); 27 | 28 | 29 | schemeData->uMode = helperOC::DynSys_UMode_Min; 30 | schemeData->dMode = helperOC::DynSys_DMode_Max; 31 | 32 | helperOC::HJIPDE_extraOuts extraOuts; 33 | 34 | extraArgs.targets = targets; 35 | extraArgs.obstacles = obstacles; 36 | 37 | helperOC::HJIPDE* hjipde; 38 | hjipde = new helperOC::HJIPDE(); 39 | 40 | beacls::FloatVec tau_out; 41 | 42 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 43 | helperOC::HJIPDE::MinWithType_None, extraArgs); 44 | printf("hello!"); 45 | // Negate result to follow STL convention 46 | for (size_t i = 0; i < tau.size(); ++i) { 47 | std::transform(datas[i].cbegin(), datas[i].cend(), datas[i].begin(), 48 | std::negate()); 49 | } 50 | 51 | if (hjipde) delete hjipde; 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Simulation/eval_u.m: -------------------------------------------------------------------------------- 1 | function [u,Vq_V,Vq_th] = eval_u(state,g,deriv_th,deriv_V,t_step,value) 2 | u = zeros(2,1); 3 | val_threshold = 0; 4 | w_mat = [-0.4,0.4]; %range of allowable turning rate 5 | a_mat = [-0.2,0.2]; %range of allowable acceleration 6 | th_eps = 0;%0.02; %deviation of values from 0 that do not warrant a change in steering angle (u=0) 7 | V_eps = 0; 8 | 9 | %Linear interpolation 10 | 11 | [xd,yd,thd,Vd,y2d] = ndgrid(linspace(g.min(1),g.max(1),g.N(1)),linspace(g.min(2),g.max(2),g.N(2)),... 12 | linspace(g.min(3),g.max(3),g.N(3)),linspace(g.min(4),g.max(4),g.N(4)),linspace(g.min(5),g.max(5),g.N(5))); 13 | 14 | [xq,yq,thq,Vq,y2q] = ndgrid(state(1),state(2),state(3),state(4),state(5)); 15 | 16 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th,xq,yq,thq,Vq,y2q); 17 | 18 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V,xq,yq,thq,Vq,y2q); 19 | 20 | if Vq_th>=-th_eps && Vq_th<=th_eps 21 | u(1) = 0; 22 | elseif Vq_th<=0 23 | if state(3)+min(w_mat)*t_step <= g.min(3) %|| value <= val_threshold 24 | u(1) = 0; 25 | else 26 | u(1) = min(w_mat); 27 | end 28 | elseif Vq_th>=0 29 | if state(3)+max(w_mat)*t_step >= g.max(3) %|| value <= val_threshold 30 | u(1) = 0; 31 | else 32 | u(1) = max(w_mat); 33 | end 34 | end 35 | 36 | if Vq_V>=-V_eps && Vq_V<=V_eps 37 | u(2) = 0; 38 | elseif Vq_V<=0 39 | if state(4)+min(a_mat)*t_step <= g.min(4) 40 | u(2) = 0; 41 | else 42 | u(2) = min(a_mat); 43 | end 44 | elseif Vq_V>=0 45 | if state(4)+max(a_mat)*t_step >= g.max(4) %|| %value <= val_threshold 46 | u(2) = 0; 47 | else 48 | u(2) = max(a_mat); 49 | end 50 | end 51 | 52 | % if value <= val_threshold 53 | % u(1:2) = 0; 54 | % end 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Car4D_Car1D_Intersection/until.cpp: -------------------------------------------------------------------------------- 1 | int until( 2 | std::vector& datas, 3 | beacls::FloatVec alpha, 4 | beacls::FloatVec beta, 5 | FLOAT_TYPE tau1, 6 | FLOAT_TYPE tau2, 7 | helperOC::DynSysSchemeData* schemeData, 8 | beacls::FloatVec tau, 9 | helperOC::HJIPDE_extraArgs extraArgs) { 10 | 11 | const FLOAT_TYPE small = 1e-3; 12 | const size_t numel = schemeData->get_grid()->get_numel(); 13 | 14 | std::vector targets(tau.size()); 15 | for (size_t i = 0; i < tau.size(); ++i) { 16 | targets[i].assign(numel, 100.); 17 | if (tau[i] > tau1 - small && tau[i] < tau2 + small) { 18 | // satisfy beta if tau1 < tau < tau2, but also negate 19 | std::transform(beta.cbegin(), beta.cend(), targets[i].begin(), 20 | std::negate()); 21 | } 22 | } 23 | 24 | std::vector obstacles(1); 25 | obstacles[0].assign(numel, 0.); 26 | std::copy(alpha.cbegin(), alpha.cend(), obstacles[0].begin()); 27 | 28 | 29 | schemeData->uMode = helperOC::DynSys_UMode_Min; 30 | schemeData->dMode = helperOC::DynSys_DMode_Max; 31 | 32 | helperOC::HJIPDE_extraOuts extraOuts; 33 | 34 | extraArgs.targets = targets; 35 | extraArgs.obstacles = obstacles; 36 | 37 | helperOC::HJIPDE* hjipde; 38 | hjipde = new helperOC::HJIPDE(); 39 | 40 | beacls::FloatVec tau_out; 41 | 42 | hjipde->solve(datas, tau_out, extraOuts, targets, tau, schemeData, 43 | helperOC::HJIPDE::MinWithType_None, extraArgs); 44 | 45 | // Negate result to follow STL convention 46 | for (size_t i = 0; i < tau.size(); ++i) { 47 | std::transform(datas[i].cbegin(), datas[i].cend(), datas[i].begin(), 48 | std::negate()); 49 | } 50 | 51 | if (hjipde) delete hjipde; 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_alpha.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_alpha( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec thetarange, 10 | beacls::FloatVec vrange, 11 | beacls::FloatVec y2range){ 12 | 13 | FLOAT_TYPE enhance; 14 | enhance = 2.; 15 | 16 | if (dim == 0){ 17 | //reset the default values 18 | std::fill(lane.begin(),lane.end(),2.); 19 | 20 | } else if (dim == 1) { 21 | // std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 22 | // [dim_long_offset, lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 23 | // return lane_i - std::pow((xs_i - .7)/1.4,2); }); 24 | 25 | } else if (dim == 2) { 26 | // std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 27 | // [theta_offset,enhance](const auto &xs_i, const auto &lane_i) { 28 | // return (lane_i - std::pow((theta_diff(xs_i,theta_offset)/(M_PI)),2)); }); 29 | 30 | for (int i = 0; i <= lane.size()-1; ++i) { 31 | if (xs[i]thetarange[1]+0.0001){ 32 | lane[i] = -3.; 33 | } 34 | } 35 | } else if (dim == 3) { 36 | for (int i = 0; i <= lane.size()-1; ++i) { 37 | if (xs[i]vrange[1]+0.0001){ 38 | lane[i] = -3.; 39 | } 40 | } 41 | } else if (dim == 4) { 42 | for (int i = 0; i <= lane.size()-1; ++i) { 43 | if (xs[i]y2range[1]+0.0001){ 44 | lane[i] = -3.;} 45 | 46 | } 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_alpha_2.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_alpha_2( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec thetarange, 10 | beacls::FloatVec vrange, 11 | beacls::FloatVec y2range, 12 | FLOAT_TYPE fill_value){ 13 | 14 | FLOAT_TYPE enhance; 15 | enhance = 3.; 16 | 17 | if (dim == 0){ 18 | //reset the default values 19 | std::fill(lane.begin(),lane.end(),fill_value); 20 | 21 | } else if (dim == 1) { 22 | // std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 23 | // [dim_long_offset, lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 24 | // return lane_i - std::pow((xs_i - .7)/1.4,2); }); 25 | 26 | } else if (dim == 2) { 27 | // std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 28 | // [theta_offset,enhance](const auto &xs_i, const auto &lane_i) { 29 | // return lane_i+0.5*(1-std::pow((theta_diff(xs_i,theta_offset)/(5./18.*M_PI)),2)); }); 30 | 31 | for (int i = 0; i <= lane.size()-1; ++i) { 32 | if (xs[i]thetarange[1]+0.0001){ 33 | lane[i] = -3.; 34 | } 35 | } 36 | } else if (dim == 3) { 37 | for (int i = 0; i <= lane.size()-1; ++i) { 38 | if (xs[i]vrange[1]+0.0001){ 39 | lane[i] = -3.; 40 | } 41 | } 42 | } else if (dim == 4) { 43 | for (int i = 0; i <= lane.size()-1; ++i) { 44 | if (xs[i]y2range[1]+0.0001){ 45 | lane[i] = -3.;} 46 | 47 | } 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Car3D_Intersection/add_sq.cpp: -------------------------------------------------------------------------------- 1 | 2 | void add_sq( 3 | beacls::FloatVec& lane, 4 | beacls::FloatVec xs, 5 | FLOAT_TYPE lane_offset, 6 | FLOAT_TYPE lane_width, 7 | FLOAT_TYPE vehicle_width, 8 | FLOAT_TYPE theta_min, 9 | FLOAT_TYPE theta_max, 10 | const size_t numel, 11 | size_t dim){ 12 | 13 | 14 | if (dim == 0){ 15 | //reset the default values 16 | for (int i = 0; itheta_max){ 46 | min_theta_diff = std::min(theta_diff(xs[i],theta_min),theta_diff(xs[i],theta_max)); 47 | lane[i] = lane[i] - 5.*std::pow(min_theta_diff/M_PI,2); 48 | 49 | } 50 | } 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Hardware Experiment/ROS_Intersection.m: -------------------------------------------------------------------------------- 1 | 2 | % clear all 3 | % global alpha_U_beta1 alpha_U_beta2 gmatOut1 gmatOut2 deriv1 deriv_R1 deriv_L1 deriv2 deriv_R2 deriv_L2 4 | % load alpha_U_beta1.mat 5 | % load alpha_U_beta2.mat 6 | % load deriv2.mat 7 | % load deriv1.mat 8 | % load derivLR2.mat 9 | % load derivLR1.mat 10 | 11 | rosinit('http://joe.local:11311'); 12 | %rosinit 13 | global ctrlpub msg 14 | 15 | ctrlpub = rospublisher('/ctrl_MATLAB','std_msgs/Float64MultiArray'); 16 | msg = rosmessage(ctrlpub); 17 | 18 | rossubscriber('/StateSpace',@ctrl_for_joe); 19 | 20 | function ctrl_for_joe(~,message) 21 | global alpha_U_beta1 alpha_U_beta2 gmatOut1 gmatOut2 deriv1 deriv_R1 deriv_L1 deriv2 deriv_R2 deriv_L2 22 | global ctrlpub msg 23 | x = message.Data(1);%message.x; 24 | y = message.Data(2);%message.y; 25 | th = message.Data(3);%message.theta; 26 | V = message.Data(4);%message.V; 27 | y2 = message.Data(5);%message.y2; 28 | t = message.Data(6); 29 | tstep_sim = 0.25; 30 | if t>12 31 | t = 12; 32 | end 33 | i = floor(t/tstep_sim); %which alpha_U_beta to evaluate 34 | 35 | traj = [x,y,th,V,y2]; 36 | value1 = 0; 37 | value2 = 0; 38 | [~,value1] = eval_u(traj,gmatOut1,alpha_U_beta1{end-i}(:,:,:,:,:),alpha_U_beta1{end-i}(:,:,:,:,:),0,value1); 39 | [~,value2] = eval_u(traj,gmatOut2,alpha_U_beta2{end-i}(:,:,:,:,:),alpha_U_beta2{end-i}(:,:,:,:,:),0,value2); 40 | 41 | %compute gradient of value function and control action 42 | if value1>value2 43 | [u,~,~] = eval_u_deriv(traj,gmatOut1,deriv1{3}(:,:,:,:,:,end-i),deriv1{4}(:,:,:,:,:,end-i),... 44 | deriv_R1{4}(:,:,:,:,:,end-i),deriv_L1{4}(:,:,:,:,:,end-i),0,value1); 45 | else 46 | [u,~,~] = eval_u_deriv(traj,gmatOut2,deriv2{3}(:,:,:,:,:,end-i),deriv2{4}(:,:,:,:,:,end-i),... 47 | deriv_R2{4}(:,:,:,:,:,end-i),deriv_L2{4}(:,:,:,:,:,end-i),0,value2); 48 | end 49 | u = [u(2),u(1)]'; 50 | u(isnan(u)) = 0; 51 | msg.Data = double(u); 52 | send(ctrlpub,msg); 53 | 54 | end 55 | 56 | %rosshutdown 57 | 58 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/ROS_Intersection.m: -------------------------------------------------------------------------------- 1 | 2 | % clear all 3 | % global alpha_U_beta1 alpha_U_beta2 gmatOut1 gmatOut2 deriv1 deriv_R1 deriv_L1 deriv2 deriv_R2 deriv_L2 4 | % load alpha_U_beta1.mat 5 | % load alpha_U_beta2.mat 6 | % load deriv2.mat 7 | % load deriv1.mat 8 | % load derivLR2.mat 9 | % load derivLR1.mat 10 | 11 | rosinit('http://joe.local:11311'); 12 | %rosinit 13 | global ctrlpub msg 14 | 15 | ctrlpub = rospublisher('/ctrl_MATLAB','std_msgs/Float64MultiArray'); 16 | msg = rosmessage(ctrlpub); 17 | 18 | rossubscriber('/StateSpace',@ctrl_for_joe); 19 | 20 | function ctrl_for_joe(~,message) 21 | global alpha_U_beta1 alpha_U_beta2 gmatOut1 gmatOut2 deriv1 deriv_R1 deriv_L1 deriv2 deriv_R2 deriv_L2 22 | global ctrlpub msg 23 | x = message.Data(1);%message.x; 24 | y = message.Data(2);%message.y; 25 | th = message.Data(3);%message.theta; 26 | V = message.Data(4);%message.V; 27 | y2 = message.Data(5);%message.y2; 28 | t = message.Data(6); 29 | tstep_sim = 0.25; 30 | if t>12 31 | t = 12; 32 | end 33 | i = floor(t/tstep_sim); %which alpha_U_beta to evaluate 34 | 35 | traj = [x,y,th,V,y2]; 36 | value1 = 0; 37 | value2 = 0; 38 | [~,value1] = eval_u(traj,gmatOut1,alpha_U_beta1{end-i}(:,:,:,:,:),alpha_U_beta1{end-i}(:,:,:,:,:),0,value1); 39 | [~,value2] = eval_u(traj,gmatOut2,alpha_U_beta2{end-i}(:,:,:,:,:),alpha_U_beta2{end-i}(:,:,:,:,:),0,value2); 40 | 41 | %compute gradient of value function and control action 42 | if value1>value2 43 | [u,~,~] = eval_u_deriv(traj,gmatOut1,deriv1{3}(:,:,:,:,:,end-i),deriv1{4}(:,:,:,:,:,end-i),... 44 | deriv_R1{4}(:,:,:,:,:,end-i),deriv_L1{4}(:,:,:,:,:,end-i),0,value1); 45 | else 46 | [u,~,~] = eval_u_deriv(traj,gmatOut2,deriv2{3}(:,:,:,:,:,end-i),deriv2{4}(:,:,:,:,:,end-i),... 47 | deriv_R2{4}(:,:,:,:,:,end-i),deriv_L2{4}(:,:,:,:,:,end-i),0,value2); 48 | end 49 | u = [u(2),u(1)]'; 50 | u(isnan(u)) = 0; 51 | msg.Data = double(u); 52 | send(ctrlpub,msg); 53 | 54 | end 55 | 56 | %rosshutdown 57 | 58 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Reachability Computation/README: -------------------------------------------------------------------------------- 1 | 2018/05/17: alpha - vector that has the values corresponding to the "Green Light" situation. 2 | beta - vector that only has positive values for the starting and target lane. 3 | 4 | %% Create_Fourway_Intersection.cpp %% 5 | - Main script that replaces Car_test.cpp in the original setup. 6 | - Calls fourway_intersection.cpp to assign values to grid points and passes a "Command" 7 | to the script (0-Green Light, 1-Red Light) 8 | 9 | %% fourway_intersection.cpp %% 10 | - Contains the parameters that define the different parts of the intersection: 11 | - theta_offset: the ideal angle at which the car should be traveling on the road section. 12 | - lane_offset: the center of the road's longitudinal dimension 13 | - range: {xmin,xmax,ymin,ymax} the vertices of the road section. 14 | - enhance: a special case used for the target lane to multiply the values of the 15 | lane vector by a factor. 16 | - Initializes the value-function vector to a low value (-12). 17 | - The last part of the script where 18 | 19 | %% get_subvector.cpp %% 20 | - Obtain the relevant subvector that relates to the road section that we are interested in. 21 | 22 | %% add_lane.cpp %% 23 | - Resets the values in the lane vector to 0 when the vector is first processed for 24 | dimension 0 (x) or dimension 1 (y). 25 | - Takes in the enhance variable to know when to multiply the lane vector by a factor. 26 | - Returns the values assigned to the lane. 27 | 28 | %% add_sq.cpp %% 29 | - Resets the values in the lane vector to 0 when the vector is first processed for 30 | dimension 0 (x). 31 | - Performs a function that outputs a lane vector that has the characteristics of running 32 | add_lane.cpp for both dim=0 and 1 then taking the max values. 33 | 34 | %% theta_diff.cpp %% 35 | - finds the minimum d_theta between the 2 input angles. 36 | 37 | 38 | 39 | 40 | Installation 41 | - In the Makefiles, make sure that the installation directories of the dependencies are correct (e.g. NVCC, INSTALL_DIR, MATIO_DIR) 42 | -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/solve_nlp.m: -------------------------------------------------------------------------------- 1 | function sol = solve_nlp(N,F,initial_state,state_min,state_max,ctrl_min,ctrl_max,state_with_time_grid,Value_function) 2 | 3 | import casadi.* 4 | 5 | % Start with an empty NLP 6 | w={}; 7 | w0 = []; 8 | lbw = []; 9 | ubw = []; 10 | J = 0; 11 | g={}; 12 | lbg = []; 13 | ubg = []; 14 | 15 | % "Lift" initial conditions 16 | Xk = MX.sym('X0', 6); 17 | w = {w{:}, Xk}; 18 | lbw = [lbw; initial_state]; 19 | ubw = [ubw; initial_state]; 20 | w0 = [w0; initial_state]; 21 | 22 | % Formulate the NLP 23 | for k=0:N-1 24 | % New NLP variable for the control 25 | Uk = MX.sym(['U_' num2str(k)],2); 26 | w = {w{:}, Uk}; 27 | lbw = [lbw; ctrl_min]; 28 | ubw = [ubw; ctrl_max]; 29 | w0 = [w0; 0; 0]; 30 | 31 | % Integrate till the end of the interval 32 | Fk = F('x0', Xk, 'p', Uk); 33 | Xk_end = Fk.xf; 34 | 35 | find_max_V = []; 36 | v_range = linspace(-0.075,0.2250,21); 37 | for v_ind = 1:21 38 | find_max_V = [find_max_V,Xk_end(1:3),v_range(v_ind),Xk_end(5:6)]; 39 | end 40 | 41 | %J=J+Fk.qf; 42 | 43 | LUT = interpolant('LUT','linear',state_with_time_grid,Value_function(:)); 44 | max(find_max 45 | %J = J + (10*Uk(1))^2 + (Uk(2)*10)^2; %+ (Xk_end(7)-Xk(7))^2 + (Xk_end(8)-Xk(8))^2; 46 | J = J - max(LUT(Xk_end(1:3)));% + (Uk(1))^2 + (Uk(2))^2; 47 | 48 | % New NLP variable for state at end of interval 49 | Xk = MX.sym(['X_' num2str(k+1)], 6); 50 | w = [w, {Xk}]; 51 | lbw = [lbw; state_min; 0]; 52 | ubw = [ubw; state_max; Inf]; 53 | w0 = [w0; initial_state]; 54 | 55 | % Add equality constraint 56 | g = [g, {Xk_end-Xk}]; 57 | lbg = [lbg; 0; 0; 0; 0; 0; 0]; 58 | ubg = [ubg; 0; 0; 0; 0; 0; 0]; 59 | end 60 | 61 | % Create an NLP solver 62 | prob = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:})); 63 | 64 | % opt.print_time = 0; 65 | % opt.verbose_init=0; 66 | % opt.ipopt.print_level = 0; 67 | solver = nlpsol('solver', 'ipopt', prob); 68 | 69 | 70 | % Solve the NLP 71 | sol = solver('x0', w0, 'lbx', lbw, 'ubx', ubw,... 72 | 'lbg', lbg, 'ubg', ubg); 73 | %w_opt = full(sol.x); -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_beta_1.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_beta_1( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec thetarange, 10 | beacls::FloatVec vrange, 11 | beacls::FloatVec y2range){ 12 | 13 | FLOAT_TYPE enhance; 14 | enhance = 2.; 15 | 16 | if (dim == 0){ 17 | //reset the default values 18 | std::fill(lane.begin(),lane.end(),0.); 19 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 20 | [lane_offset, lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 21 | return lane_i + enhance*(1-std::pow(((xs_i - lane_offset)/((lane_width-vehicle_width)/2.)),2)); }); 22 | 23 | } else if (dim == 1) { 24 | // std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 25 | // [dim_long_offset, lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 26 | // return lane_i - std::pow((xs_i - .7)/1.4,2); }); 27 | 28 | } else if (dim == 2) { 29 | // std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 30 | // [theta_offset,enhance](const auto &xs_i, const auto &lane_i) { 31 | // return (lane_i - std::pow((theta_diff(xs_i,theta_offset)/(M_PI)),2)); }); 32 | 33 | for (int i = 0; i <= lane.size()-1; ++i) { 34 | if (xs[i]thetarange[1]+0.0001){ 35 | lane[i] = -3.; 36 | } 37 | } 38 | } else if (dim == 3) { 39 | for (int i = 0; i <= lane.size()-1; ++i) { 40 | if (xs[i]vrange[1]+0.0001){ 41 | lane[i] = -3.; 42 | } 43 | } 44 | } else if (dim == 4) { 45 | for (int i = 0; i <= lane.size()-1; ++i) { 46 | if (xs[i]y2range[1]+0.0001){ 47 | lane[i] = -3.;} 48 | 49 | } 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Simulation/RunData.m: -------------------------------------------------------------------------------- 1 | clear all 2 | load beta.mat 3 | close all 4 | [gmat,datamat] = cpp2matG(g,{data}); 5 | [gmat2] = processGrid(gmat); 6 | % %% 7 | % % 3D visualization of Target Set Values 8 | % figure 9 | % visSetIm(gmat2,datamat,'r',0); %x,y,th 10 | 11 | % 2D Projection 12 | close all 13 | th_mat = [0:2*pi/8:2*pi]; 14 | max_caxis = max(datamat(:)); 15 | min_caxis = min(datamat(:)); 16 | th = pi; 17 | V = 0.1; 18 | y2 = -0.5; 19 | for i = 1 20 | [g2d,data2d] = proj(gmat2,datamat,[0,0,1,1,1],[th,V,y2]); 21 | %[g2d,data2d] = proj(gmat2,datamat,[0,0,1,1,1],'max'); 22 | %visSetIm(g2d,data2d,'r'); 23 | % Contour plot (y vs.x) 24 | figure; %subplot(3,3,i) 25 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 26 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 27 | [~,h]=contourf(X,Y,data2d',[-100:0.1:10]); 28 | colorbar; 29 | caxismin = 0.3; 30 | caxismax = 2; 31 | caxis([caxismin caxismax]); 32 | hold on; 33 | [~,h2] = contour(X,Y,data2d',[0 0],'ShowText','on'); 34 | set(h2,'LineColor','k'); 35 | xlabel('x (m)') 36 | ylabel('y (m)') 37 | title(['x-y plane at \theta=' num2str(th*180/pi) '^o']) 38 | %title('alpha') 39 | %title(['Max Projection on x-y Plane']) 40 | 41 | set(h,'LineColor','none'); 42 | axis([-0.6 0.6 -0.6 0.6]) 43 | set(gcf, 'Position', [100 44 936 790]) 44 | ax = gca; 45 | ax.FontSize = 20; 46 | end 47 | %% Contour plot (theta vs. x) Road_test.mat - Single Lane 48 | close all 49 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 50 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 51 | contour(X,Y,data2d',[-7:0.5:0.5],'ShowText','on') 52 | xlabel('x') 53 | ylabel('\theta') 54 | hold on 55 | h1 = plot([-4,4],[pi/2,pi/2],'--k') 56 | h2 = plot([2,2],[g2d.min(2),g2d.max(2)],'k') 57 | plot([-2,-2],[g2d.min(2),g2d.max(2)],'k') 58 | legend([h1,h2],'\theta offset','road boundary') 59 | 60 | %% Contour plot (y vs. x) Road_test.mat - Single Lane 61 | close all 62 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 63 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 64 | levels = -6:0.5:0.5; 65 | contour(X,Y,data2d',levels,'ShowText','on') 66 | xlabel('x') 67 | ylabel('y') 68 | hold on 69 | %h1 = plot([-4,4],[pi/2,pi/2],'--k') 70 | h2 = plot([2,2],[g2d.min(2),g2d.max(2)],'k') 71 | plot([-2,-2],[g2d.min(2),g2d.max(2)],'k') 72 | legend(h2,'road boundary') -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Simulation/eval_u_deriv.m: -------------------------------------------------------------------------------- 1 | function [u,Vq_V,Vq_th] = eval_u_deriv(state,g,deriv_th,deriv_V,deriv_V_R,deriv_V_L,t_step,value) 2 | u = zeros(2,1); 3 | 4 | vrange = [0,0.15]; %range of acceptable velocitie 5 | w_mat = [-0.4,0.4]; %range of allowable turning rate 6 | a_mat = [-0.2,0.2]; %range of allowable acceleration 7 | v_mat = linspace(g.min(4),g.max(4),g.N(4)); 8 | v_thresh_min = min(v_mat(v_mat>=vrange(1))); 9 | v_thresh_max = max(v_mat(v_mat<=vrange(2))); 10 | 11 | th_eps = 0;%0.02; %deviation of values from 0 that do not warrant a change in steering angle (u=0) 12 | V_eps = 0; 13 | 14 | %Linear interpolation 15 | 16 | [xd,yd,thd,Vd,y2d] = ndgrid(linspace(g.min(1),g.max(1),g.N(1)),linspace(g.min(2),g.max(2),g.N(2)),... 17 | linspace(g.min(3),g.max(3),g.N(3)),linspace(g.min(4),g.max(4),g.N(4)),linspace(g.min(5),g.max(5),g.N(5))); 18 | 19 | [xq,yq,thq,Vq,y2q] = ndgrid(state(1),state(2),state(3),state(4),state(5)); 20 | 21 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th,xq,yq,thq,Vq,y2q); 22 | 23 | if Vq>=vrange(1) && Vq<=v_thresh_min 24 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V_R,xq,yq,thq,Vq,y2q); 25 | elseif Vq<=vrange(2) && Vq>=v_thresh_max 26 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V_L,xq,yq,thq,Vq,y2q); 27 | else 28 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V,xq,yq,thq,Vq,y2q); 29 | end 30 | 31 | 32 | if Vq_th>=-th_eps && Vq_th<=th_eps 33 | u(1) = 0; 34 | elseif Vq_th<=0 35 | if state(3)+min(w_mat)*t_step <= g.min(3) %|| value <= val_threshold 36 | u(1) = 0; 37 | else 38 | u(1) = min(w_mat); 39 | end 40 | elseif Vq_th>=0 41 | if state(3)+max(w_mat)*t_step >= g.max(3) %|| value <= val_threshold 42 | u(1) = 0; 43 | else 44 | u(1) = max(w_mat); 45 | end 46 | end 47 | 48 | if Vq_V>=-V_eps && Vq_V<=V_eps 49 | u(2) = 0; 50 | elseif Vq_V<=0 51 | if state(4)+min(a_mat)*t_step <= g.min(4) 52 | u(2) = 0; 53 | else 54 | u(2) = min(a_mat); 55 | end 56 | elseif Vq_V>=0 57 | if state(4)+max(a_mat)*t_step >= g.max(4) %|| %value <= val_threshold 58 | u(2) = 0; 59 | else 60 | u(2) = max(a_mat); 61 | end 62 | end 63 | 64 | 65 | % if value <= val_threshold 66 | % u(1:2) = 0; 67 | % end 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/ComputeGradient_C.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | void ComputeGradient_C( 5 | std::vector& derivC_th_mat, 6 | std::vector& derivC_vel_mat, 7 | std::vector& derivC_y2_mat, 8 | const levelset::HJI_Grid* g, 9 | const std::vector& data, 10 | const size_t tau_length) 11 | { 12 | 13 | const bool calculateTTRduringSolving = false; 14 | bool useCuda = false; 15 | int num_of_threads = 0; 16 | int num_of_gpus = 0; 17 | size_t line_length_of_chunk = 1; 18 | bool enable_user_defined_dynamics_on_gpu = true; 19 | 20 | helperOC::HJIPDE_extraArgs extraArgs; 21 | levelset::DelayedDerivMinMax_Type delayedDerivMinMax = levelset::DelayedDerivMinMax_Disable; 22 | extraArgs.execParameters.line_length_of_chunk = line_length_of_chunk; 23 | extraArgs.execParameters.calcTTR = calculateTTRduringSolving; 24 | extraArgs.execParameters.useCuda = useCuda; 25 | extraArgs.execParameters.num_of_gpus = num_of_gpus; 26 | extraArgs.execParameters.num_of_threads = num_of_threads; 27 | extraArgs.execParameters.delayedDerivMinMax = delayedDerivMinMax; 28 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = 29 | enable_user_defined_dynamics_on_gpu; 30 | const helperOC::ExecParameters execParameters = extraArgs.execParameters; 31 | const beacls::UVecType execType = (execParameters.useCuda) ? beacls::UVecType_Cuda : beacls::UVecType_Vector; 32 | //bool computeGradients = new ComputeGradients(g, helperOC::ApproximationAccuracy_veryHigh, execType); 33 | 34 | ComputeGradients* computeGradients; 35 | computeGradients = new ComputeGradients(g, helperOC::ApproximationAccuracy_veryHigh, execType); 36 | beacls::FloatVec BRS_at_t; 37 | 38 | std::vector derivC; 39 | std::vector derivL; 40 | std::vector derivR; 41 | 42 | for(size_t i = 0; i < tau_length; ++i){ 43 | BRS_at_t = data[i]; 44 | computeGradients->operator()(derivC, derivL, derivR, g, data[i], data[i].size(), false, execParameters); 45 | derivC_th_mat.push_back(derivC[2]); 46 | derivC_vel_mat.push_back(derivC[3]); 47 | derivC_y2_mat.push_back(derivC[4]); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/ComputeHyperplanceCoefficients.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | void ComputeGradient_C( 5 | std::vector& derivC_th_mat, 6 | std::vector& derivC_vel_mat, 7 | std::vector& derivC_y2_mat, 8 | const levelset::HJI_Grid* g, 9 | const std::vector& data, 10 | const size_t tau_length) 11 | { 12 | 13 | const bool calculateTTRduringSolving = false; 14 | bool useCuda = false; 15 | int num_of_threads = 0; 16 | int num_of_gpus = 0; 17 | size_t line_length_of_chunk = 1; 18 | bool enable_user_defined_dynamics_on_gpu = true; 19 | 20 | helperOC::HJIPDE_extraArgs extraArgs; 21 | levelset::DelayedDerivMinMax_Type delayedDerivMinMax = levelset::DelayedDerivMinMax_Disable; 22 | extraArgs.execParameters.line_length_of_chunk = line_length_of_chunk; 23 | extraArgs.execParameters.calcTTR = calculateTTRduringSolving; 24 | extraArgs.execParameters.useCuda = useCuda; 25 | extraArgs.execParameters.num_of_gpus = num_of_gpus; 26 | extraArgs.execParameters.num_of_threads = num_of_threads; 27 | extraArgs.execParameters.delayedDerivMinMax = delayedDerivMinMax; 28 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = 29 | enable_user_defined_dynamics_on_gpu; 30 | const helperOC::ExecParameters execParameters = extraArgs.execParameters; 31 | const beacls::UVecType execType = (execParameters.useCuda) ? beacls::UVecType_Cuda : beacls::UVecType_Vector; 32 | //bool computeGradients = new ComputeGradients(g, helperOC::ApproximationAccuracy_veryHigh, execType); 33 | 34 | ComputeGradients* computeGradients; 35 | computeGradients = new ComputeGradients(g, helperOC::ApproximationAccuracy_veryHigh, execType); 36 | beacls::FloatVec BRS_at_t; 37 | 38 | std::vector derivC; 39 | std::vector derivL; 40 | std::vector derivR; 41 | 42 | for(size_t i = 0; i < tau_length; ++i){ 43 | BRS_at_t = data[i]; 44 | computeGradients->operator()(derivC, derivL, derivR, g, data[i], data[i].size(), false, execParameters); 45 | derivC_th_mat.push_back(derivC[2]); 46 | derivC_vel_mat.push_back(derivC[3]); 47 | derivC_y2_mat.push_back(derivC[4]); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/RunData.m: -------------------------------------------------------------------------------- 1 | clear all 2 | load alpha.mat 3 | %close all 4 | [gmat,datamat] = cpp2matG(g,{data}); 5 | [gmat2] = processGrid(gmat); 6 | % %% 7 | % % 3D visualization of Target Set Values 8 | % figure 9 | % visSetIm(gmat2,datamat,'r',0); %x,y,th 10 | 11 | % 2D Projection 12 | %close all 13 | th_mat = [0:2*pi/8:2*pi]; 14 | max_caxis = max(datamat(:)); 15 | min_caxis = min(datamat(:)); 16 | %th = 5*pi/4; 17 | V = 0.1; 18 | y2 = -0.6; 19 | figure; 20 | for i = 1:numel(th_mat)-1 21 | subplot(2,4,i); 22 | th = th_mat(i); 23 | [g2d,data2d] = proj(gmat2,datamat,[0,0,1,1,1],[th_mat(i),V,0.4]); 24 | %[g2d,data2d] = proj(gmat2,datamat,[0,0,1,1,1],'max'); 25 | %visSetIm(g2d,data2d,'r'); 26 | % Contour plot (y vs.x) 27 | %subplot13,3,i) 28 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 29 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 30 | [~,h]=contourf(X,Y,data2d',[-100:0.1:10]); 31 | colorbar; 32 | caxismin = 0.3; 33 | caxismax = 2; 34 | %caxis([caxismin caxismax]); 35 | hold on; 36 | [~,h2] = contour(X,Y,data2d',[0 0],'ShowText','on'); 37 | set(h2,'LineColor','k'); 38 | xlabel('x (m)') 39 | ylabel('y (m)') 40 | title(['x-y plane at \theta=' num2str(th*180/pi) '^o']) 41 | %title('alpha') 42 | %title(['Max Projection on x-y Plane']) 43 | 44 | set(h,'LineColor','none'); 45 | %axis([-0.6 0.6 -0.6 0.6]) 46 | %set(gcf, 'Position', [100 44 936 790]) 47 | set(gcf,'Position',[113 -66 1732 549]) 48 | %ax = gca; 49 | %ax.FontSize = 20; 50 | end 51 | %% Contour plot (theta vs. x) Road_test.mat - Single Lane 52 | close all 53 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 54 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 55 | contour(X,Y,data2d',[-7:0.5:0.5],'ShowText','on') 56 | xlabel('x') 57 | ylabel('\theta') 58 | hold on 59 | h1 = plot([-4,4],[pi/2,pi/2],'--k') 60 | h2 = plot([2,2],[g2d.min(2),g2d.max(2)],'k') 61 | plot([-2,-2],[g2d.min(2),g2d.max(2)],'k') 62 | legend([h1,h2],'\theta offset','road boundary') 63 | 64 | %% Contour plot (y vs. x) Road_test.mat - Single Lane 65 | close all 66 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 67 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 68 | levels = -6:0.5:0.5; 69 | contour(X,Y,data2d',levels,'ShowText','on') 70 | xlabel('x') 71 | ylabel('y') 72 | hold on 73 | %h1 = plot([-4,4],[pi/2,pi/2],'--k') 74 | h2 = plot([2,2],[g2d.min(2),g2d.max(2)],'k') 75 | plot([-2,-2],[g2d.min(2),g2d.max(2)],'k') 76 | legend(h2,'road boundary') -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_beta_2.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_beta_2top( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec thetarange, 10 | beacls::FloatVec vrange, 11 | beacls::FloatVec y2range){ 12 | 13 | FLOAT_TYPE enhance; 14 | enhance = 2.; 15 | 16 | if (dim == 0){ 17 | //reset the default values 18 | std::fill(lane.begin(),lane.end(),0.); 19 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 20 | [lane_offset, lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 21 | return enhance*(1-std::pow(((xs_i - lane_offset)/((lane_width-vehicle_width)/2.)),2)); }); 22 | 23 | } else if (dim == 1) { 24 | beacls::FloatVec lane_temp = lane; 25 | 26 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 27 | [lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 28 | return lane_i*(1-std::pow((xs_i - .8)/1.7,2)); }); 29 | 30 | // std::transform(lane_temp.cbegin(), lane_temp.cend(), lane.begin(), lane.begin(), 31 | // [lane_width, vehicle_width, enhance](const auto &lane_temp_i, const auto &lane_i) { 32 | // return std::max(lane_temp_i,lane_i); }); 33 | 34 | } else if (dim == 2) { 35 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 36 | [theta_offset,enhance](const auto &xs_i, const auto &lane_i) { 37 | return lane_i*(1-std::pow((theta_diff(xs_i,theta_offset)/(5./18.*M_PI)),2)); }); 38 | 39 | for (int i = 0; i <= lane.size()-1; ++i) { 40 | if (xs[i]thetarange[1]+0.0001){ 41 | lane[i] = -10.; 42 | } 43 | } 44 | } else if (dim == 3) { 45 | for (int i = 0; i <= lane.size()-1; ++i) { 46 | if (xs[i]vrange[1]){ 47 | lane[i] = -10.; 48 | } 49 | } 50 | } else if (dim == 4) { 51 | for (int i = 0; i <= lane.size()-1; ++i) { 52 | if (xs[i]y2range[1]){ 53 | lane[i] = -10.;} 54 | 55 | } 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_beta_2bot.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_beta_2bot( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec thetarange, 10 | beacls::FloatVec vrange, 11 | beacls::FloatVec y2range){ 12 | 13 | FLOAT_TYPE enhance; 14 | enhance = 2.; 15 | 16 | if (dim == 0){ 17 | //reset the default values 18 | std::fill(lane.begin(),lane.end(),0.); 19 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 20 | [lane_offset, lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 21 | return enhance*(1-std::pow(((xs_i - lane_offset)/((lane_width-vehicle_width)/2.)),4)); }); 22 | 23 | } else if (dim == 1) { 24 | beacls::FloatVec lane_temp = lane; 25 | 26 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 27 | [lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 28 | return lane_i*(1-std::pow((xs_i + .6)/1.,2)); }); 29 | 30 | // std::transform(lane_temp.cbegin(), lane_temp.cend(), lane.begin(), lane.begin(), 31 | // [lane_width, vehicle_width, enhance](const auto &lane_temp_i, const auto &lane_i) { 32 | // return std::max(lane_temp_i,lane_i); }); 33 | 34 | } else if (dim == 2) { 35 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 36 | [theta_offset,enhance](const auto &xs_i, const auto &lane_i) { 37 | return lane_i*(1-std::pow((theta_diff(xs_i,theta_offset)/(5./18.*M_PI)),2)); }); 38 | 39 | for (int i = 0; i <= lane.size()-1; ++i) { 40 | if (xs[i]thetarange[1]+0.0001){ 41 | lane[i] = -3.; 42 | } 43 | } 44 | } else if (dim == 3) { 45 | for (int i = 0; i <= lane.size()-1; ++i) { 46 | if (xs[i]vrange[1]+0.0001){ 47 | lane[i] = -3.; 48 | } 49 | } 50 | } else if (dim == 4) { 51 | for (int i = 0; i <= lane.size()-1; ++i) { 52 | if (xs[i]y2range[1]+0.0001){ 53 | lane[i] = -3.;} 54 | 55 | } 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_beta_2top.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_beta_2top( 2 | beacls::FloatVec& lane, 3 | beacls::FloatVec xs, 4 | FLOAT_TYPE lane_offset, 5 | FLOAT_TYPE lane_width, 6 | FLOAT_TYPE theta_offset, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim, 9 | beacls::FloatVec thetarange, 10 | beacls::FloatVec vrange, 11 | beacls::FloatVec y2range){ 12 | 13 | FLOAT_TYPE enhance; 14 | enhance = 3.; 15 | 16 | if (dim == 0){ 17 | //reset the default values 18 | std::fill(lane.begin(),lane.end(),0.); 19 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 20 | [lane_offset, lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 21 | return enhance*(1-std::pow(((xs_i - lane_offset)/((lane_width-vehicle_width)/2.)),2)); }); 22 | 23 | } else if (dim == 1) { 24 | beacls::FloatVec lane_temp = lane; 25 | 26 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 27 | [lane_width, vehicle_width, enhance](const auto &xs_i, const auto &lane_i) { 28 | return lane_i*(1-std::pow((xs_i - .6)/1.2,2)); }); 29 | 30 | // std::transform(lane_temp.cbegin(), lane_temp.cend(), lane.begin(), lane.begin(), 31 | // [lane_width, vehicle_width, enhance](const auto &lane_temp_i, const auto &lane_i) { 32 | // return std::max(lane_temp_i,lane_i); }); 33 | 34 | } else if (dim == 2) { 35 | std::transform(xs.cbegin(), xs.cend(), lane.begin(), lane.begin(), 36 | [theta_offset,enhance](const auto &xs_i, const auto &lane_i) { 37 | return lane_i*(1-std::pow((theta_diff(xs_i,theta_offset)/(5./18.*M_PI)),2)); }); 38 | 39 | for (int i = 0; i <= lane.size()-1; ++i) { 40 | if (xs[i]thetarange[1]+0.0001){ 41 | lane[i] = -3.; 42 | } 43 | } 44 | } else if (dim == 3) { 45 | for (int i = 0; i <= lane.size()-1; ++i) { 46 | if (xs[i]vrange[1]+0.0001){ 47 | lane[i] = -3.; 48 | } 49 | } 50 | } else if (dim == 4) { 51 | for (int i = 0; i <= lane.size()-1; ++i) { 52 | if (xs[i]y2range[1]+0.0001){ 53 | lane[i] = -3.;} 54 | 55 | } 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/loadOptimalStates.m: -------------------------------------------------------------------------------- 1 | function [OptStates, MaxValueFunction] = loadOptimalStates(states,V1,V2,p,thd,vd,v2,integration,integration_iters) 2 | 3 | OptStates.x_bar = []; 4 | OptStates.y_bar = []; 5 | OptStates.th_bar = []; 6 | OptStates.v_bar = []; 7 | 8 | t = linspace(states(6),states(6)+p.T,p.N+1); 9 | dt = p.T/p.N; 10 | %states_New = zeros(6,1); 11 | 12 | for i = 1:numel(t) 13 | 14 | single_OptStates = loadSingleOptimalStates(states,V1,thd,vd); 15 | if i == 1 16 | MaxValueFunction = 1; 17 | end 18 | 19 | omega = (single_OptStates(1) - states(3))/dt; 20 | if omega < p.omegaMin 21 | omega = p.omegaMin; 22 | elseif omega > p.omegaMax 23 | omega = p.omegaMax; 24 | end 25 | 26 | accel = (single_OptStates(2) - states(4))/dt; 27 | if accel < p.accelMin 28 | accel = p.accelMin; 29 | elseif accel > p.accelMax 30 | accel = p.accelMax; 31 | end 32 | 33 | dt2 = dt/integration_iters; 34 | if integration == 0 %Euler 35 | for i = 1:integration_iters 36 | states(1) = states(1) + states(4)*cos(states(3))*dt2; 37 | states(2) = states(2) + (states(4)*sin(states(3))-0.01)*dt2; 38 | states(3) = states(3) + omega*dt2; 39 | states(4) = states(4) + accel*dt2; 40 | states(5) = states(5) + v2*dt2; 41 | states(6) = states(6) + dt2; 42 | OptStates.th_bar = [OptStates.th_bar, single_OptStates(1)]; 43 | OptStates.v_bar = [OptStates.v_bar, single_OptStates(2)]; 44 | end 45 | elseif integration == 1 %Heun 46 | old_th = states(3); 47 | old_v = states(4); 48 | states(3) = states(3) + omega*dt; 49 | states(4) = states(4) + accel*dt; 50 | states(1) = states(1) + states(4)*cos(states(3))*dt/2 + old_v*cos(old_th)*dt/2; 51 | states(2) = states(2) + states(4)*sin(states(3))*dt/2 + old_v*sin(old_th)*dt/2; 52 | states(5) = states(5) + v2*dt; 53 | states(6) = states(6) + dt; 54 | OptStates.th_bar = [OptStates.th_bar, single_OptStates(1)]; 55 | OptStates.v_bar = [OptStates.v_bar, single_OptStates(2)]; 56 | end 57 | 58 | OptStates.x_bar = [OptStates.x_bar, states(1)]; 59 | OptStates.y_bar = [OptStates.y_bar, states(2)]; 60 | end 61 | OptStates.th_bar = OptStates.th_bar(1:(numel(t)-1)*integration_iters+1); 62 | OptStates.v_bar = OptStates.v_bar(1:(numel(t)-1)*integration_iters+1); 63 | 64 | end 65 | 66 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Numerical Simulation/RunData.m: -------------------------------------------------------------------------------- 1 | clear all 2 | %load overtake_output.mat 3 | load overtake_output.mat 4 | %% 5 | close all 6 | [gmat,datamat] = cpp2matG(g,{alpha_2}); 7 | [gmat2] = processGrid(gmat); 8 | % %% 9 | % % 3D visualization of Target Set Values 10 | % figure 11 | % visSetIm(gmat2,datamat,'r',0); %x,y,th 12 | 13 | % 2D Projection 14 | %close all 15 | th_mat = linspace(g.min(3),g.max(3),10); 16 | max_caxis = max(datamat(:)); 17 | min_caxis = min(datamat(:)); 18 | %th = 5*pi/4 19 | V = 0.05; 20 | %y2 = -0.5; 21 | y2=-0.; 22 | figure; 23 | for i = 1:numel(th_mat) 24 | subplot(2,5,i); 25 | th = th_mat(i); 26 | [g2d,data2d] = proj(gmat2,datamat,[0,0,1,1,1],[th_mat(i),V,y2]); 27 | %[g2d,data2d] = proj(gmat2,datamat,[0,0,1,1,1],'max'); 28 | %visSetIm(g2d,data2d,'r'); 29 | % Contour plot (y vs.x) 30 | %subplot13,3,i) 31 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 32 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 33 | [~,h]=contourf(X,Y,data2d',[-10:0.1:3.]); 34 | colorbar; 35 | caxismin = -3; 36 | caxismax = 3; 37 | caxis([caxismin caxismax]); 38 | hold on; 39 | [~,h2] = contour(X,Y,data2d',[0 0],'ShowText','on'); 40 | set(h2,'LineColor','k'); 41 | xlabel('x (m)') 42 | ylabel('y (m)') 43 | title(['x-y plane at \theta=' num2str(th*180/pi) '^o']) 44 | %title('alpha') 45 | %title(['Max Projection on x-y Plane']) 46 | 47 | set(h,'LineColor','none'); 48 | axis equal 49 | %axis([-0.4 0.4 -0.6 0.6]) 50 | %set(gcf, 'Position', [100 44 936 790]) 51 | 52 | set(gcf,'Position',[113 -66 1732 549]) 53 | 54 | %ax = gca; 55 | %ax.FontSize = 20; 56 | end 57 | %% Contour plot (theta vs. x) Road_test.mat - Single Lane 58 | close all 59 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 60 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 61 | contour(X,Y,data2d',[-7:0.5:0.5],'ShowText','on') 62 | xlabel('x') 63 | ylabel('\theta') 64 | hold on 65 | h1 = plot([-4,4],[pi/2,pi/2],'--k') 66 | h2 = plot([2,2],[g2d.min(2),g2d.max(2)],'k') 67 | plot([-2,-2],[g2d.min(2),g2d.max(2)],'k') 68 | legend([h1,h2],'\theta offset','road boundary') 69 | 70 | %% Contour plot (y vs. x) Road_test.mat - Single Lane 71 | close all 72 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 73 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 74 | levels = -6:0.5:0.5; 75 | contour(X,Y,data2d',levels,'ShowText','on') 76 | xlabel('x') 77 | ylabel('y') 78 | hold on 79 | %h1 = plot([-4,4],[pi/2,pi/2],'--k') 80 | h2 = plot([2,2],[g2d.min(2),g2d.max(2)],'k') 81 | plot([-2,-2],[g2d.min(2),g2d.max(2)],'k') 82 | legend(h2,'road boundary') -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/MPC_video.m: -------------------------------------------------------------------------------- 1 | close all; h3 = figure; hold on; 2 | 3 | 4 | v = VideoWriter('Test.avi'); 5 | v.FrameRate = 10; 6 | open(v) 7 | 8 | xlabel('x (m)') 9 | ylabel('y (m)') 10 | %title(['\theta=' num2str(traj{t_mat(k)}(end,3)*180/pi,'%.0f') '^o, t=' num2str(((t_mat(k)-1)*T)/(length(alpha_U_beta)-1),'%.0f') 's']) 11 | %set(h,'LineColor','none'); 12 | axis equal 13 | axis([-0.4 0.4 -0.6 0.6]) 14 | 15 | set(gcf, 'Position', [100 44 936 790]) 16 | ax = gca; 17 | ax.FontSize = 20; 18 | 19 | [X,Y] = ndgrid(xd,yd); 20 | Z = zeros(numel(xd),numel(yd)); 21 | 22 | [~,h] = contourf(X,Y,Z,[-100:.1:10]); 23 | %colormap pink 24 | colorbar; 25 | caxis([-3,2.5]) 26 | set(h,'LineColor','none'); 27 | 28 | [~,h2] = contour(X,Y,Z,[0,0],'k'); 29 | 30 | rho_scale = 0.1; 31 | rho = 0.2; 32 | q = quiver(0,0,0,0); 33 | q.Color = 'black'; 34 | q.LineWidth = 2; 35 | q.MaxHeadSize = 1; 36 | q.AutoScale = 'off'; 37 | 38 | rho2 = rho_scale*0.15/0.15 + 0.1; 39 | base_y2 = traj(1,5); 40 | q2 = quiver(0,0,0,0); 41 | q2.Color = 'black'; 42 | q2.LineWidth = 2; 43 | q2.MaxHeadSize = 1; 44 | q2.AutoScale = 'off'; 45 | V1 = griddedInterpolant({xd,yd,thd,vd,y2d,td},overtake_output_Ext); 46 | 47 | %t_iter = floor(numel(tau)/120); 48 | 49 | for i = 1:numel(tau) 50 | 51 | delete( findobj(gca, 'type', 'line') ); 52 | title([num2str(tau(i),'%.2f') 's']) 53 | 54 | 55 | Z = V1({xd,yd,traj(i,3),traj(i,4),traj(i,5),traj(i,6)}); 56 | 57 | 58 | 59 | %[~,h] = contourf(X,Y,Z); 60 | set(h,'ZData',Z); 61 | set(h2,'ZData',Z); 62 | %plot([0.2,-0.4],[-0.4,0.2],'xk','Markersize',15,'LineWidth',6); 63 | if traj(i,5)<0.7 64 | h5=plot(-0.2,traj(i,5),'ok','MarkerSize',30,'MarkerFaceColor','red','LineWidth',8); 65 | end 66 | 67 | h4=plot(traj(i,1),traj(i,2),'ok','MarkerSize',30,'MarkerFaceColor','white','LineWidth',8); 68 | 69 | base_x = traj(i,1); 70 | base_y = traj(i,2); 71 | set(q,'xdata',base_x,'ydata',base_y,'udata',rho*cos(traj(i,3)),'vdata',rho*sin(traj(i,3))) 72 | 73 | if traj(i,5)<0.7 74 | base_y2 = traj(i,5); 75 | set(q2,'xdata',-0.2,'ydata',base_y2,'udata',0,'vdata',rho2) 76 | else 77 | set(q2,'xdata',-0.2,'ydata',base_y2,'udata',0,'vdata',0) 78 | end 79 | 80 | set(gca,'xcolor','w','ycolor','w','xtick',[],'ytick',[]) 81 | frame=getframe(gcf); 82 | writeVideo(v,frame); 83 | 84 | end 85 | close(v) -------------------------------------------------------------------------------- /Old Files/Car3D_Intersection/Simulation/Simulation.m: -------------------------------------------------------------------------------- 1 | clear all 2 | load alpha_U_beta.mat 3 | [gmat,datamat] = cpp2matG(g,alpha_U_beta); 4 | gmatOut = processGrid(gmat,datamat(:,:,:,1)); 5 | deriv = computeGradients(gmatOut,datamat); 6 | save('deriv.mat','deriv','gmatOut') 7 | 8 | %% 9 | % clear all 10 | % close all 11 | % load alpha_U_beta.mat 12 | % load deriv.mat 13 | T = 3; %total time 14 | tstep = T/(length(alpha_U_beta)-1); %timestep between aUb frames 15 | tstep_sim = tstep/5; %timestep between control evaluations 16 | t_real = 0:tstep:T; %s 17 | t_sim = 0:tstep_sim:T; 18 | u = zeros(numel(t_real),1); 19 | state = zeros(numel(t_sim),3); 20 | traj = cell(numel(t_real),1); 21 | traj_temp = [2,-7,pi/2]; 22 | traj{1} = traj_temp; %initial state 23 | 24 | t = cell(numel(t_real),1); 25 | t_temp = 0; 26 | t{1} = t_temp; 27 | 28 | for i = 1:numel(t_real)-1 29 | for j = 1:tstep/tstep_sim 30 | u(i) = eval_u(traj_temp(end,:),gmatOut,{deriv{1}(:,:,:,end+1-i);deriv{2}(:,:,:,end+1-i);deriv{3}(:,:,:,end+1-i)}); 31 | [t_temp,traj_temp] = ode45(@(t_temp,traj_temp) odefun_dubinsCar(t_temp,traj_temp,u(i)),... 32 | [t_temp(end) t_temp(end)+tstep_sim],traj_temp(end,:)); 33 | t{i} = [t{i};t_temp(2:end)]; 34 | traj{i} = [traj{i};traj_temp(2:end,:)]; 35 | if traj_temp(end,1)>=12 || traj_temp(end,1)<=-12 || traj_temp(end,2)>=12 || traj_temp(end,2)<=-12 36 | break 37 | end 38 | end 39 | end 40 | 41 | %% 42 | load alpha_U_beta.mat 43 | t_mat = [1:3:10,12,13]; 44 | 45 | for k = 1:numel(t_mat) 46 | [gmat,datamat] = cpp2matG(g,alpha_U_beta(t_mat(k))); 47 | [gmat2] = processGrid(gmat); 48 | [g2d,data2d] = proj(gmat2,datamat,[0,0,1],'max'); 49 | subplot(2,3,7-k) 50 | X = linspace(g2d.min(1),g2d.max(1),g2d.N(1)); 51 | Y = linspace(g2d.min(2),g2d.max(2),g2d.N(2)); 52 | [~,h]=contourf(X,Y,data2d',[-100:0.1:10]); 53 | colorbar; 54 | hold on; 55 | [~,h2] = contour(X,Y,data2d',[0 0],'ShowText','on'); 56 | set(h2,'LineColor','k'); 57 | xlabel('x (m)') 58 | ylabel('y (m)') 59 | title(['y vs. x at \theta=[0,2\pi] t=' num2str((-(t_mat(k)-1)*5)/20) 's']) 60 | set(h,'LineColor','none'); 61 | axis([-10 10 -10 10]) 62 | end 63 | 64 | for k = 2:numel(t_mat) 65 | subplot(2,3,k); hold on; 66 | if k>= i 67 | max_j = i; 68 | else 69 | max_j = t_mat(k); 70 | end 71 | 72 | for j = 1:max_j-1 73 | plot(traj{j}(:,1),traj{j}(:,2),'k','LineWidth',2) 74 | end 75 | end 76 | 77 | %% 78 | % subplot(2,3,1); hold on; 79 | % for j = 1:max_j-1 80 | % plot(traj{j}(:,1),traj{j}(:,2),'k','LineWidth',2) 81 | % end 82 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Numerical Simulation/computeGradients_C.m: -------------------------------------------------------------------------------- 1 | function derivC = computeGradients_C(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | %derivL = cell(g.dim, 1); 31 | %derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | derivC{i} = zeros(size(data),'single'); 55 | %derivL{i} = zeros(size(data),'single'); 56 | %derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL, derivR] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [derivL, derivR] = derivFunc(g, data(colons{:}, t), i); 68 | derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/computeGradients_C.m: -------------------------------------------------------------------------------- 1 | function derivC = computeGradients_C(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | %derivL = cell(g.dim, 1); 31 | %derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | derivC{i} = zeros(size(data),'single'); 55 | %derivL{i} = zeros(size(data),'single'); 56 | %derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL, derivR] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [derivL, derivR] = derivFunc(g, data(colons{:}, t), i); 68 | derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Numerical Simulation/computeGradients_L.m: -------------------------------------------------------------------------------- 1 | function derivL = computeGradients_L(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | derivL = cell(g.dim, 1); 31 | %derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | %derivC{i} = zeros(size(data),'single'); 55 | derivL{i} = zeros(size(data),'single'); 56 | %derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL{i}, ~] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | %derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [derivL{i}(colons{:},t), ~] = derivFunc(g, data(colons{:}, t), i); 68 | %derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Simulation/computeGradients_C.m: -------------------------------------------------------------------------------- 1 | function derivC = computeGradients_C(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | %derivL = cell(g.dim, 1); 31 | %derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | derivC{i} = zeros(size(data),'single'); 55 | %derivL{i} = zeros(size(data),'single'); 56 | %derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL, derivR] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [derivL, derivR] = derivFunc(g, data(colons{:}, t), i); 68 | derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/computeGradients_L.m: -------------------------------------------------------------------------------- 1 | function derivL = computeGradients_L(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | derivL = cell(g.dim, 1); 31 | %derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | %derivC{i} = zeros(size(data),'single'); 55 | derivL{i} = zeros(size(data),'single'); 56 | %derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL{i}, ~] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | %derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [derivL{i}(colons{:},t), ~] = derivFunc(g, data(colons{:}, t), i); 68 | %derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/computeGradients_R.m: -------------------------------------------------------------------------------- 1 | function derivR = computeGradients_R(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | derivL = cell(g.dim, 1); 31 | derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | %derivC{i} = zeros(size(data),'single'); 55 | %derivL{i} = zeros(size(data),'single'); 56 | derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL{i}, derivR] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | %derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [~, derivR{i}(colons{:},t)] = derivFunc(g, data(colons{:}, t), i); 68 | %derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Numerical Simulation/computeGradients_R.m: -------------------------------------------------------------------------------- 1 | function derivR = computeGradients_R(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | derivL = cell(g.dim, 1); 31 | derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | %derivC{i} = zeros(size(data),'single'); 55 | %derivL{i} = zeros(size(data),'single'); 56 | derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL{i}, derivR] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | %derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [~, derivR{i}(colons{:},t)] = derivFunc(g, data(colons{:}, t), i); 68 | %derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Simulation/computeGradients_L.m: -------------------------------------------------------------------------------- 1 | function derivL = computeGradients_L(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | derivL = cell(g.dim, 1); 31 | %derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | %derivC{i} = zeros(size(data),'single'); 55 | derivL{i} = zeros(size(data),'single'); 56 | %derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL{i}, ~] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | %derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [derivL{i}(colons{:},t), ~] = derivFunc(g, data(colons{:}, t), i); 68 | %derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Numerical Simulation/computeGradients.m: -------------------------------------------------------------------------------- 1 | function [derivC, derivL, derivR] = computeGradients_C(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | derivL = cell(g.dim, 1); 31 | derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | %derivC{i} = zeros(size(data),'single'); 55 | derivL{i} = zeros(size(data),'single'); 56 | %derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL{i}, derivR] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | %derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [derivL{i}, derivR] = derivFunc(g, data(colons{:}, t), i); 68 | %derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Simulation/computeGradients_R.m: -------------------------------------------------------------------------------- 1 | function derivR = computeGradients_R(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | derivL = cell(g.dim, 1); 31 | derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | %derivC{i} = zeros(size(data),'single'); 55 | %derivL{i} = zeros(size(data),'single'); 56 | derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL{i}, derivR] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | %derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [~, derivR{i}(colons{:},t)] = derivFunc(g, data(colons{:}, t), i); 68 | %derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/loadOptimalStates.m: -------------------------------------------------------------------------------- 1 | function [OptStates, MaxValueFunction] = loadOptimalStates(states,V1,V2,p,thd,vd,v2,integration,integration_iters) 2 | 3 | OptStates.x_bar = []; 4 | OptStates.y_bar = []; 5 | OptStates.th_bar = []; 6 | OptStates.v_bar = []; 7 | 8 | t = linspace(states(6),states(6)+p.T,p.N+1); 9 | dt = p.T/p.N; 10 | %states_New = zeros(6,1); 11 | 12 | for i = 1:numel(t) 13 | 14 | if V1(states)>=V2(states) 15 | single_OptStates = loadSingleOptimalStates(states,V1,thd,vd); 16 | if i == 1 17 | MaxValueFunction = 1; 18 | end 19 | else 20 | single_OptStates = loadSingleOptimalStates(states,V2,thd,vd); 21 | if i == 1 22 | MaxValueFunction = 2; 23 | end 24 | end 25 | 26 | omega = (single_OptStates(1) - states(3))/dt; 27 | if omega < p.omegaMin 28 | omega = p.omegaMin; 29 | elseif omega > p.omegaMax 30 | omega = p.omegaMax; 31 | end 32 | 33 | accel = (single_OptStates(2) - states(4))/dt; 34 | if accel < p.accelMin 35 | accel = p.accelMin; 36 | elseif accel > p.accelMax 37 | accel = p.accelMax; 38 | end 39 | 40 | dt2 = dt/integration_iters; 41 | if integration == 0 %Euler 42 | for i = 1:integration_iters 43 | states(1) = states(1) + states(4)*cos(states(3))*dt2; 44 | states(2) = states(2) + states(4)*sin(states(3))*dt2; 45 | states(3) = states(3) + omega*dt2; 46 | states(4) = states(4) + accel*dt2; 47 | states(5) = states(5) + v2*dt2; 48 | states(6) = states(6) + dt2; 49 | OptStates.th_bar = [OptStates.th_bar, single_OptStates(1)]; 50 | OptStates.v_bar = [OptStates.v_bar, single_OptStates(2)]; 51 | end 52 | elseif integration == 1 %Heun 53 | old_th = states(3); 54 | old_v = states(4); 55 | states(3) = states(3) + omega*dt; 56 | states(4) = states(4) + accel*dt; 57 | states(1) = states(1) + states(4)*cos(states(3))*dt/2 + old_v*cos(old_th)*dt/2; 58 | states(2) = states(2) + states(4)*sin(states(3))*dt/2 + old_v*sin(old_th)*dt/2; 59 | states(5) = states(5) + v2*dt; 60 | states(6) = states(6) + dt; 61 | OptStates.th_bar = [OptStates.th_bar, single_OptStates(1)]; 62 | OptStates.v_bar = [OptStates.v_bar, single_OptStates(2)]; 63 | end 64 | 65 | OptStates.x_bar = [OptStates.x_bar, states(1)]; 66 | OptStates.y_bar = [OptStates.y_bar, states(2)]; 67 | end 68 | OptStates.th_bar = OptStates.th_bar(1:(numel(t)-1)*integration_iters+1); 69 | OptStates.v_bar = OptStates.v_bar(1:(numel(t)-1)*integration_iters+1); 70 | 71 | end 72 | 73 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Simulation/computeGradients.m: -------------------------------------------------------------------------------- 1 | function [derivC, derivL, derivR] = computeGradients_C(g, data, dims, derivFunc) 2 | % [derivC, derivL, derivR] = computeGradients(g, data, derivFunc, upWind) 3 | % 4 | % Estimates the costate p at position x for cost function data on grid g by 5 | % numerically taking partial derivatives along each grid direction. 6 | % Numerical derivatives are taken using the levelset toolbox 7 | % 8 | % Inputs: grid - grid structure 9 | % data - array of g.dim dimensions containing function values 10 | % derivFunc - derivative approximation function (from level set 11 | % toolbox) 12 | % upWind - whether to use upwinding (ignored; to be implemented in 13 | % the future 14 | % 15 | % Output: derivC - (central) gradient in a g.dim by 1 vector 16 | % derivL - left gradient 17 | % derivR - right gradient 18 | 19 | if nargin < 3 20 | dims = true(g.dim, 1); 21 | end 22 | 23 | if nargin < 4 24 | derivFunc = @upwindFirstWENO5; 25 | end 26 | 27 | 28 | % Go through each dimension and compute the gradient in each 29 | derivC = cell(g.dim, 1); 30 | derivL = cell(g.dim, 1); 31 | derivR = cell(g.dim, 1); 32 | 33 | if numDims(data) == g.dim 34 | tau_length = 1; 35 | elseif numDims(data) == g.dim + 1 36 | tau_length = size(data); 37 | tau_length = tau_length(end); 38 | colons = repmat({':'}, 1, g.dim); 39 | else 40 | error('Dimensions of input data and grid don''t match!') 41 | end 42 | 43 | % Just in case there are NaN values in the data (usually from TTR functions) 44 | numInfty = 1e6; 45 | nanInds = isnan(data); 46 | data(nanInds) = numInfty; 47 | 48 | % Just in case there are inf values 49 | infInds = isinf(data); 50 | data(infInds) = numInfty; 51 | 52 | for i = 1:g.dim 53 | if dims(i) 54 | %derivC{i} = zeros(size(data),'single'); 55 | derivL{i} = zeros(size(data),'single'); 56 | %derivR{i} = zeros(size(data),'single'); 57 | %% data at a single time stamp 58 | if tau_length == 1 59 | % Compute gradient using level set toolbox 60 | [derivL{i}, derivR] = derivFunc(g, data, i); 61 | 62 | % Central gradient 63 | %derivC{i} = (0.5*(derivL + derivR)); 64 | else 65 | %% data at multiple time stamps 66 | for t = 1:tau_length 67 | [derivL{i}, derivR] = derivFunc(g, data(colons{:}, t), i); 68 | %derivC{i}(colons{:}, t) = 0.5*(derivL + derivR); 69 | end 70 | end 71 | 72 | % Change indices where data was nan to nan 73 | derivC{i}(nanInds) = nan; 74 | 75 | % Change indices where data was inf to inf 76 | derivC{i}(infInds) = inf; 77 | end 78 | end 79 | end -------------------------------------------------------------------------------- /MPC Controller/Intersection/Yalmip-Matlab/MPC_video.m: -------------------------------------------------------------------------------- 1 | close all; h3 = figure; hold on; 2 | 3 | 4 | v = VideoWriter('Test.avi'); 5 | v.FrameRate = 10; 6 | open(v) 7 | 8 | xlabel('x (m)') 9 | ylabel('y (m)') 10 | %title(['\theta=' num2str(traj{t_mat(k)}(end,3)*180/pi,'%.0f') '^o, t=' num2str(((t_mat(k)-1)*T)/(length(alpha_U_beta)-1),'%.0f') 's']) 11 | %set(h,'LineColor','none'); 12 | axis([-0.6 0.6 -0.6 0.6]) 13 | 14 | set(gcf, 'Position', [100 44 936 790]) 15 | ax = gca; 16 | ax.FontSize = 20; 17 | 18 | [X,Y] = ndgrid(xd,yd); 19 | Z = zeros(numel(xd),numel(yd)); 20 | 21 | [~,h] = contourf(X,Y,Z,[-100:.1:10]); 22 | %colormap pink 23 | colorbar; 24 | caxis([-12,2.5]) 25 | set(h,'LineColor','none'); 26 | 27 | [~,h2] = contour(X,Y,Z,[0,0],'k'); 28 | 29 | rho_scale = 0.1; 30 | rho = 0.2; 31 | q = quiver(0,0,0,0); 32 | q.Color = 'black'; 33 | q.LineWidth = 2; 34 | q.MaxHeadSize = 1; 35 | q.AutoScale = 'off'; 36 | 37 | rho2 = -rho_scale*0.15/0.15 - 0.1; 38 | base_y2 = traj(1,5); 39 | q2 = quiver(0,0,0,0); 40 | q2.Color = 'black'; 41 | q2.LineWidth = 2; 42 | q2.MaxHeadSize = 1; 43 | q2.AutoScale = 'off'; 44 | V1 = griddedInterpolant({xd,yd,thd,vd,y2d,td},alpha_U_beta1_Ext); 45 | V2 = griddedInterpolant({xd,yd,thd,vd,y2d,td},alpha_U_beta2_Ext); 46 | %t_iter = floor(numel(tau)/120); 47 | 48 | for i = 1:numel(tau) 49 | 50 | delete( findobj(gca, 'type', 'line') ); 51 | title([num2str(tau(i),'%.2f') 's']) 52 | 53 | if MaxValueFunction(i) == 1 54 | Z = V1({xd,yd,traj(i,3),traj(i,4),traj(i,5),traj(i,6)}); 55 | else 56 | Z = V2({xd,yd,traj(i,3),traj(i,4),traj(i,5),traj(i,6)}); 57 | end 58 | %[~,h] = contourf(X,Y,Z); 59 | set(h,'ZData',Z); 60 | set(h2,'ZData',Z); 61 | plot([0.2,-0.4],[-0.4,0.2],'xk','Markersize',15,'LineWidth',6); 62 | if traj(i,5)>-0.55 63 | h5=plot(-0.2,traj(i,5),'ok','MarkerSize',30,'MarkerFaceColor','red','LineWidth',8); 64 | end 65 | 66 | h4=plot(traj(i,1),traj(i,2),'ok','MarkerSize',30,'MarkerFaceColor','white','LineWidth',8); 67 | 68 | base_x = traj(i,1); 69 | base_y = traj(i,2); 70 | set(q,'xdata',base_x,'ydata',base_y,'udata',rho*cos(traj(i,3)),'vdata',rho*sin(traj(i,3))) 71 | 72 | if traj(i,5)>-0.55 73 | base_y2 = traj(i,5); 74 | set(q2,'xdata',-0.2,'ydata',base_y2,'udata',0,'vdata',rho2) 75 | else 76 | set(q2,'xdata',-0.2,'ydata',base_y2,'udata',0,'vdata',0) 77 | end 78 | 79 | set(gca,'xcolor','w','ycolor','w','xtick',[],'ytick',[]) 80 | frame=getframe(gcf); 81 | writeVideo(v,frame); 82 | 83 | end 84 | close(v) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # STL-HJ 2 | Code for “Signal Temporal Logic meets Hamilton-Jacobi Reachability: Connections and Applications” by Mo Chen, Qizhan Tam, Scott C. Livingston, and Marco Pavone: 3 | http://asl/wp-content/papercite-data/pdf/Chen.Tam.Livingston.Pavone.WAFR18.pdf. 4 | 5 | ## Installation 6 | 1. HJ Reachability Computations (C++, CUDA): 7 | - The computation uses the beacls toolbox, which can be found here: https://github.com/HJReachability/beacls/tree/master/sources. 8 | - In the Makefiles, make sure that the installation directories of the dependencies are correct (e.g. NVCC, INSTALL_DIR, etc.). 9 | - Currently, the STL functions are not compatible with the CUDA option. 10 | 11 | 2. Numerical Simulations (Matlab): 12 | - Some functions used are from the helperOC toolbox: https://github.com/HJReachability/helperOC.git 13 | - Using the helperOC toolbox requires: https://bitbucket.org/ian_mitchell/toolboxls 14 | 15 | 3. Hardware Experiments (ROS Kinetic, Python, Matlab): 16 | - Turtlebot 3 burger is used as the hardware platform. 17 | - ROS files derived from: https://github.com/StanfordASL/asl_turtlebot (as of 5 Sept 2018). 18 | - For switching controller, Matlab is required. 19 | 20 | 4. MPC Controller (IPOPT, Casadi-Python, Yalmip-Matlab): 21 | - An improved alternative to the switching controller typically used in HJ Reachability. 22 | - Implemented using Yalmip-Matlab and Casadi-Python as parsers, both use IPOPT as the solver. 23 | - For numerical simulations and visualization, install Yalmip: https://yalmip.github.io/tutorial/installation/. 24 | - For hardware experiments install Casadi: https://github.com/casadi/casadi/wiki/InstallationInstructions. 25 | - Note: LFRCS partially implemented for highway overtaking example. 26 | 27 | ## Folders 28 | 1. Turtlebot_Car4D_Car1D_Intersection: 29 | - Traffic light intersection example as described in the paper linked above. 30 | - Contains files for reachability computation, numerical simulations, and implementation on Turtlebots. 31 | 32 | 2. Turtlebot_Car4D_Car1D_Overtake: 33 | - Highway overtaking example as described in the paper linked above. 34 | - Contains files for reachability computation, numerical simulations, and implementation on Turtlebots. 35 | 36 | 3. STL Functions: 37 | - Contains files for temporal operators (i.e. always, eventually, until). 38 | 39 | 4. Test Sample: 40 | - A quick computation to test whether the installation was successful. 41 | 42 | 5. MPC Controller: 43 | - Contains Yalmip-Matlab implementation for numerical simulations, Casadi-Python implementation for (ROS) hardware experiments. 44 | 45 | 6. Old Files: 46 | - Contains work done leading up to the Traffic light intersection and Highway overtaking examples. 47 | - Varied approach to the problems posed by the examples. 48 | -------------------------------------------------------------------------------- /Old Files/Car4D_Car1D_Intersection/add_lane_disturbance.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_disturbance( 2 | beacls::FloatVec& lane, 3 | const std::vector shape, 4 | beacls::FloatVec range, 5 | beacls::FloatVec gmin, 6 | beacls::FloatVec gmax, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | long unsigned int x_size, y_size, z_size, a_size, b_size, y, z, a, b, b2y, yz_unit; 20 | 21 | FLOAT_TYPE y_add = (y_unit*vehicle_width); 22 | // printf("y_unit=%f\n",y_unit); 23 | // printf("y_add=%f\n",y_add); 24 | FLOAT_TYPE x_add = (x_unit*vehicle_width); 25 | // printf("x_unit=%f\n",x_unit); 26 | // printf("x_add=%f\n",x_add); 27 | 28 | beacls::IntegerVec n; 29 | x_size = x2-x1+1; 30 | // printf("x_size=%lu\n",x_size); 31 | y_size = y2-y1+1; 32 | z_size = shape[2]; 33 | a_size = shape[3]; 34 | b_size = shape[4]; 35 | FLOAT_TYPE by_unit = static_cast(b_size)/y_size; 36 | beacls::IntegerVec b2y_index; 37 | beacls::IntegerVec y_addvec{0,0}; 38 | beacls::IntegerVec x_addvec{(long unsigned int)((static_cast(x_size)-1.)/2.-x_add/2.-0.5),(long unsigned int)((static_cast(x_size)-1.)/2.+x_add/2.+0.5)}; 39 | // printf("x_addvec=[%lu,%lu]\n",x_addvec[0],x_addvec[1]); 40 | 41 | for (b = 0; b < b_size; ++b) { 42 | b2y = (long unsigned int)(b/by_unit+0.5); 43 | b2y_index.push_back(b2y); 44 | } 45 | 46 | for (b = 0; b < b_size; ++b) { 47 | for (a = 0; a < a_size; ++a) { 48 | for(z = 0; z < z_size; ++z) { 49 | y_addvec[0] = (long unsigned int)(b2y_index[b]-y_add/2.-0.5); 50 | y_addvec[1] = (long unsigned int)(b2y_index[b]+y_add/2.+0.5); 51 | 52 | if (y_addvec[0]gmax[1]){ 56 | y_addvec[1] == gmax[1]; 57 | } 58 | for (y = y_addvec[0]; y <= y_addvec[1]; ++y){ 59 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[1]}; 60 | std::fill(lane.begin()+n[0],lane.begin()+n[1],-5.); 61 | } 62 | } 63 | } 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_disturbance.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_disturbance( 2 | beacls::FloatVec& lane, 3 | const std::vector shape, 4 | beacls::FloatVec range, 5 | beacls::FloatVec gmin, 6 | beacls::FloatVec gmax, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | long unsigned int x_size, y_size, z_size, a_size, b_size, y, z, a, b, b2y, yz_unit; 20 | 21 | FLOAT_TYPE y_add = (y_unit*vehicle_width*0.78); 22 | // printf("y_unit=%f\n",y_unit); 23 | // printf("y_add=%f\n",y_add); 24 | FLOAT_TYPE x_add = (x_unit*vehicle_width); 25 | // printf("x_unit=%f\n",x_unit); 26 | // printf("x_add=%f\n",x_add); 27 | 28 | beacls::IntegerVec n; 29 | x_size = x2-x1+1; 30 | printf("x_size=%lu\n",x_size); 31 | y_size = y2-y1+1; 32 | z_size = shape[2]; 33 | a_size = shape[3]; 34 | b_size = shape[4]; 35 | FLOAT_TYPE by_unit = static_cast(b_size)/y_size; 36 | beacls::IntegerVec b2y_index; 37 | beacls::IntegerVec y_addvec{0,0}; 38 | beacls::IntegerVec x_addvec{(long unsigned int)((static_cast(x_size)-1.)/2.-x_add/2.+0.5),(long unsigned int)((static_cast(x_size)-1.)/2.+x_add/2.+0.5)}; 39 | printf("x_addvec=[%lu,%lu]\n",x_addvec[0],x_addvec[1]); 40 | 41 | for (b = 0; b < b_size; ++b) { 42 | b2y = (long unsigned int)(b/by_unit+0.5); 43 | b2y_index.push_back(b2y); 44 | } 45 | 46 | for (b = 0; b < b_size; ++b) { 47 | for (a = 0; a < a_size; ++a) { 48 | for(z = 0; z < z_size; ++z) { 49 | y_addvec[0] = (long unsigned int)(b2y_index[b]-y_add/2.+0.5); 50 | y_addvec[1] = (long unsigned int)(b2y_index[b]+y_add/2.+0.5); 51 | 52 | if (y_addvec[0]gmax[1]){ 56 | y_addvec[1] == gmax[1]; 57 | } 58 | for (y = y_addvec[0]; y <= y_addvec[1]; ++y){ 59 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[1]}; 60 | std::fill(lane.begin()+n[0],lane.begin()+n[1],-10.); 61 | } 62 | } 63 | } 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Intersection/Reachability Computation/add_lane_disturbance.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_disturbance( 2 | beacls::FloatVec& lane, 3 | const std::vector shape, 4 | beacls::FloatVec range, 5 | beacls::FloatVec gmin, 6 | beacls::FloatVec gmax, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | long unsigned int x_size, y_size, z_size, a_size, b_size, y, z, a, b, b2y, yz_unit; 20 | 21 | FLOAT_TYPE y_add = (y_unit*vehicle_width*0.78); 22 | // printf("y_unit=%f\n",y_unit); 23 | // printf("y_add=%f\n",y_add); 24 | FLOAT_TYPE x_add = (x_unit*vehicle_width); 25 | // printf("x_unit=%f\n",x_unit); 26 | // printf("x_add=%f\n",x_add); 27 | 28 | beacls::IntegerVec n; 29 | x_size = x2-x1+1; 30 | printf("x_size=%lu\n",x_size); 31 | y_size = y2-y1+1; 32 | z_size = shape[2]; 33 | a_size = shape[3]; 34 | b_size = shape[4]; 35 | FLOAT_TYPE by_unit = static_cast(b_size)/y_size; 36 | beacls::IntegerVec b2y_index; 37 | beacls::IntegerVec y_addvec{0,0}; 38 | beacls::IntegerVec x_addvec{(long unsigned int)((static_cast(x_size)-1.)/2.-x_add/2.+0.5),(long unsigned int)((static_cast(x_size)-1.)/2.+x_add/2.+0.5)}; 39 | printf("x_addvec=[%lu,%lu]\n",x_addvec[0],x_addvec[1]); 40 | 41 | for (b = 0; b < b_size; ++b) { 42 | b2y = (long unsigned int)(b/by_unit+0.5); 43 | b2y_index.push_back(b2y); 44 | } 45 | 46 | for (b = 0; b < b_size; ++b) { 47 | for (a = 0; a < a_size; ++a) { 48 | for(z = 0; z < z_size; ++z) { 49 | y_addvec[0] = (long unsigned int)(b2y_index[b]-y_add/2.+0.5); 50 | y_addvec[1] = (long unsigned int)(b2y_index[b]+y_add/2.+0.5); 51 | 52 | if (y_addvec[0]gmax[1]){ 56 | y_addvec[1] == gmax[1]; 57 | } 58 | for (y = y_addvec[0]; y <= y_addvec[1]; ++y){ 59 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[1]}; 60 | std::fill(lane.begin()+n[0],lane.begin()+n[1],-10.); 61 | } 62 | } 63 | } 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /Old Files/AA290_work/Turtlebot_Car4D_Car1D_Intersection/Car4D_Car1D_Intersection/add_lane_disturbance.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_disturbance( 2 | beacls::FloatVec& lane, 3 | const std::vector shape, 4 | beacls::FloatVec range, 5 | beacls::FloatVec gmin, 6 | beacls::FloatVec gmax, 7 | FLOAT_TYPE vehicle_width, 8 | size_t dim){ 9 | 10 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1)/(gmax[0]-gmin[0]); 11 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 12 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 13 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 14 | beacls::IntegerVec x_index{x1,x2}; 15 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 16 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 17 | beacls::IntegerVec y_index{y1,y2}; 18 | 19 | long unsigned int x_size, y_size, z_size, a_size, b_size, y, z, a, b, b2y, yz_unit; 20 | 21 | FLOAT_TYPE y_add = (y_unit*vehicle_width*0.78); 22 | // printf("y_unit=%f\n",y_unit); 23 | // printf("y_add=%f\n",y_add); 24 | FLOAT_TYPE x_add = (x_unit*vehicle_width); 25 | // printf("x_unit=%f\n",x_unit); 26 | // printf("x_add=%f\n",x_add); 27 | 28 | beacls::IntegerVec n; 29 | x_size = x2-x1+1; 30 | printf("x_size=%lu\n",x_size); 31 | y_size = y2-y1+1; 32 | z_size = shape[2]; 33 | a_size = shape[3]; 34 | b_size = shape[4]; 35 | FLOAT_TYPE by_unit = static_cast(b_size)/y_size; 36 | beacls::IntegerVec b2y_index; 37 | beacls::IntegerVec y_addvec{0,0}; 38 | beacls::IntegerVec x_addvec{(long unsigned int)((static_cast(x_size)-1.)/2.-x_add/2.-0.5+1),(long unsigned int)((static_cast(x_size)-1.)/2.+x_add/2.+0.5+1)}; 39 | printf("x_addvec=[%lu,%lu]\n",x_addvec[0],x_addvec[1]); 40 | 41 | for (b = 0; b < b_size; ++b) { 42 | b2y = (long unsigned int)(b/by_unit+0.5); 43 | b2y_index.push_back(b2y); 44 | } 45 | 46 | for (b = 0; b < b_size; ++b) { 47 | for (a = 0; a < a_size; ++a) { 48 | for(z = 0; z < z_size; ++z) { 49 | y_addvec[0] = (long unsigned int)(b2y_index[b]-y_add/2.-0.5); 50 | y_addvec[1] = (long unsigned int)(b2y_index[b]+y_add/2.+0.5); 51 | 52 | if (y_addvec[0]gmax[1]){ 56 | y_addvec[1] == gmax[1]; 57 | } 58 | for (y = y_addvec[0]; y <= y_addvec[1]; ++y){ 59 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[1]}; 60 | std::fill(lane.begin()+n[0],lane.begin()+n[1],-5.); 61 | } 62 | } 63 | } 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/ComputeCompatibleController.cpp: -------------------------------------------------------------------------------- 1 | void ComputeHyperplaneCoefficients( 2 | std::vector& a, 3 | std::vector& b, 4 | const levelset::HJI_Grid* g, 5 | const std::vector& data, 6 | const beacls::FloatVec& v2Range) 7 | { 8 | 9 | const bool calculateTTRduringSolving = false; 10 | bool useCuda = false; 11 | int num_of_threads = 0; 12 | int num_of_gpus = 0; 13 | size_t line_length_of_chunk = 1; 14 | bool enable_user_defined_dynamics_on_gpu = true; 15 | 16 | helperOC::HJIPDE_extraArgs extraArgs; 17 | levelset::DelayedDerivMinMax_Type delayedDerivMinMax = levelset::DelayedDerivMinMax_Disable; 18 | extraArgs.execParameters.line_length_of_chunk = line_length_of_chunk; 19 | extraArgs.execParameters.calcTTR = calculateTTRduringSolving; 20 | extraArgs.execParameters.useCuda = useCuda; 21 | extraArgs.execParameters.num_of_gpus = num_of_gpus; 22 | extraArgs.execParameters.num_of_threads = num_of_threads; 23 | extraArgs.execParameters.delayedDerivMinMax = delayedDerivMinMax; 24 | extraArgs.execParameters.enable_user_defined_dynamics_on_gpu = 25 | enable_user_defined_dynamics_on_gpu; 26 | const helperOC::ExecParameters execParameters = extraArgs.execParameters; 27 | const beacls::UVecType execType = (execParameters.useCuda) ? beacls::UVecType_Cuda : beacls::UVecType_Vector; 28 | //bool computeGradients = new ComputeGradients(g, helperOC::ApproximationAccuracy_veryHigh, execType); 29 | 30 | ComputeGradients* computeGradients; 31 | computeGradients = new ComputeGradients(g, helperOC::ApproximationAccuracy_veryHigh, execType); 32 | beacls::FloatVec BRS_at_t; 33 | 34 | std::vector derivC; 35 | std::vector derivL; 36 | std::vector derivR; 37 | std::vector derivC_th; 38 | std::vector derivC_vel; 39 | std::vector derivC_y2; 40 | const size_t tau_length = data.size(); 41 | 42 | //compute derivatives 43 | for(size_t i = 0; i < tau_length; ++i){ 44 | BRS_at_t = data[i]; 45 | computeGradients->operator()(derivC, derivL, derivR, g, data[i], data[i].size(), false, execParameters); 46 | derivC_th.push_back(derivC[2]); 47 | derivC_vel.push_back(derivC[3]); 48 | derivC_y2.push_back(derivC[4]); 49 | } 50 | 51 | //find coefficients 52 | FLOAT_TYPE dOpt, d_vel, d_th, d_y2; 53 | for(size_t tau = 0; tau < tau_length; ++tau){ 54 | for(size_t state = 0; state < derivC_th[tau].size(); ++state){ 55 | //u3 = -P4/P3*u4 - P5/P3*y2 56 | d_vel = derivC_vel[tau][state]; 57 | d_th = derivC_th[tau][state]; 58 | d_y2 = derivC_y2[tau][state]; 59 | 60 | a[tau].push_back(d_vel/d_th); 61 | 62 | dOpt = (d_y2 >= 0) ? v2Range[1] : v2Range[0]; 63 | b[tau].push_back(dOpt*d_y2/d_th); 64 | } 65 | } 66 | 67 | derivC_th.clear(); 68 | derivC_vel.clear(); 69 | derivC_y2.clear(); 70 | 71 | } 72 | -------------------------------------------------------------------------------- /Turtlebot_Car4D_Car1D_Overtake/Reachability Computation/add_lane_disturbance_v2.cpp: -------------------------------------------------------------------------------- 1 | void add_lane_disturbance_v2( 2 | beacls::FloatVec& lane, 3 | const std::vector shape, 4 | beacls::FloatVec range, 5 | beacls::FloatVec gmin, 6 | beacls::FloatVec gmax, 7 | FLOAT_TYPE vehicle_width, 8 | FLOAT_TYPE fill_Value, 9 | size_t dim){ 10 | 11 | FLOAT_TYPE x_unit = (static_cast(shape[0])-1)/(gmax[0]-gmin[0]); 12 | FLOAT_TYPE y_unit = (static_cast(shape[1])-1)/(gmax[1]-gmin[1]); 13 | FLOAT_TYPE b_unit = (static_cast(shape[4])-1)/(gmax[4]-gmin[4]); 14 | long unsigned int x1 = (long unsigned int)(x_unit*(range[0]-gmin[0])+0.5); 15 | long unsigned int x2 = (long unsigned int)(x_unit*(range[1]-gmin[0])+0.5); 16 | beacls::IntegerVec x_index{x1,x2}; 17 | long unsigned int y1 = (long unsigned int)(y_unit*(range[2]-gmin[1])+0.5); 18 | long unsigned int y2 = (long unsigned int)(y_unit*(range[3]-gmin[1])+0.5); 19 | beacls::IntegerVec y_index{y1,y2}; 20 | 21 | long unsigned int x_size, y_size, z_size, a_size, b_size, y, z, a, b, b2y, yz_unit; 22 | 23 | FLOAT_TYPE y_add = (y_unit*vehicle_width); 24 | FLOAT_TYPE x_add = (x_unit*vehicle_width); 25 | 26 | beacls::IntegerVec n; 27 | x_size = x2-x1+1; 28 | y_size = y2-y1+1; 29 | z_size = shape[2]; 30 | a_size = shape[3]; 31 | b_size = shape[4]; 32 | 33 | // printf("x_size: %lu\n", x_size); 34 | // printf("y_size: %lu\n", y_size); 35 | 36 | 37 | 38 | beacls::IntegerVec b2y_index; 39 | beacls::IntegerVec y_addvec{0,0}; 40 | beacls::IntegerVec x_addvec{(long unsigned int)((static_cast(x_size)-1.)/2.-x_add+0.5),(long unsigned int)((static_cast(x_size)-1.)/2.+x_add+0.5)}; 41 | beacls::IntegerVec b_addvec{(long unsigned int)((range[2]-gmin[4])*b_unit+0.5),(long unsigned int)((range[3]-gmin[4])*b_unit+0.5)}; 42 | // printf("b_addvec: %lu\n", b_addvec[0]); 43 | // printf("b_addvec: %lu\n", b_addvec[1]); 44 | b_addvec[1] = b_addvec[1]; 45 | 46 | FLOAT_TYPE by_unit = 1.;//static_cast(b_addvec[1]-b_addvec[0]+1)/y_size; 47 | 48 | for (b = b_addvec[0]; b < b_addvec[1]+1; ++b) { 49 | for (a = 0; a < a_size; ++a) { 50 | for (z = 0; z < z_size; ++z) { 51 | y_addvec[0] = (long unsigned int)((b-b_addvec[0])/by_unit-y_add+0.5); 52 | y_addvec[1] = (long unsigned int)((b-b_addvec[0])/by_unit+y_add+0.5); 53 | // printf("y_addvec: %lu\n", y_addvec[0]); 54 | // printf("y_addvec: %lu\n", y_addvec[1]); 55 | if (y_addvec[0]<0 || y_addvec[0]>1000){ 56 | y_addvec[0] = 0; 57 | } 58 | if (y_addvec[1]>y_size-1){ 59 | y_addvec[1] = y_size-1; 60 | } 61 | 62 | for (y = y_addvec[0]; y < y_addvec[1]+1; ++y){ 63 | n = {b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[0], b*a_size*z_size*y_size*x_size + a*z_size*y_size*x_size + z*y_size*x_size + y*x_size + x_addvec[1]}; 64 | std::fill(lane.begin()+n[0],lane.begin()+n[1]+1,fill_Value); 65 | } 66 | } 67 | } 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /MPC Controller/Overtake/Yalmip-Matlab/eval_u_deriv.m: -------------------------------------------------------------------------------- 1 | function [u,Vq_V,Vq_th] = eval_u_deriv(state,g,deriv_C,deriv_R,deriv_L,t_step,value,i) 2 | u = zeros(2,1); 3 | 4 | deriv_th = deriv_C{3}(:,:,:,:,:,end+2-i); 5 | deriv_V = deriv_C{4}(:,:,:,:,:,end+2-i); 6 | deriv_th_R = deriv_R{3}(:,:,:,:,:,end+2-i); 7 | deriv_V_R = deriv_R{4}(:,:,:,:,:,end+2-i); 8 | deriv_th_L = deriv_L{3}(:,:,:,:,:,end+2-i); 9 | deriv_V_L = deriv_L{4}(:,:,:,:,:,end+2-i); 10 | 11 | vrange = [0,0.11]; %range of acceptable velocities 12 | thrange = [40*pi/180,140*pi/180]; 13 | w_mat = [-1.,1.]; %range of allowable turning rate 14 | a_mat = [-0.2,0.2]; %range of allowable acceleration 15 | v_mat = linspace(g.min(4),g.max(4),g.N(4)); 16 | th_mat = linspace(g.min(3),g.max(3),g.N(3)); 17 | v_thresh_min = min(v_mat(v_mat>vrange(1))); 18 | v_thresh_max = max(v_mat(v_mat=vrange(1) && Vq<=v_thresh_min 37 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V_R,xq,yq,thq,Vq,y2q); 38 | elseif Vq<=vrange(2) && Vq>=v_thresh_max 39 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V_L,xq,yq,thq,Vq,y2q); 40 | else 41 | Vq_V = interpn(xd,yd,thd,Vd,y2d,deriv_V,xq,yq,thq,Vq,y2q); 42 | end 43 | 44 | if thq>=thrange(1) && thq<=th_thresh_min 45 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th_R,xq,yq,thq,Vq,y2q); 46 | elseif thq<=thrange(2) && thq>=th_thresh_max 47 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th_L,xq,yq,thq,Vq,y2q); 48 | elseif thq>=pi/2 && thq<=th_thresh_min2 49 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th_R,xq,yq,thq,Vq,y2q); 50 | elseif thq<=pi/2 && thq>=th_thresh_max2 51 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th_L,xq,yq,thq,Vq,y2q); 52 | else 53 | Vq_th = interpn(xd,yd,thd,Vd,y2d,deriv_th,xq,yq,thq,Vq,y2q); 54 | end 55 | 56 | 57 | % if Vq_th>=-th_eps && Vq_th<=th_eps 58 | % u(1) = 0; 59 | if Vq_th<=0 60 | if state(3)+min(w_mat)*t_step <= g.min(3) || abs(Vq_th) < 1e-5 %|| value <= val_threshold 61 | u(1) = 0; 62 | else 63 | u(1) = min(w_mat); 64 | end 65 | elseif Vq_th>=0 66 | if state(3)+max(w_mat)*t_step >= g.max(3) %|| value <= val_threshold 67 | u(1) = 0; 68 | else 69 | u(1) = max(w_mat); 70 | end 71 | end 72 | 73 | % if Vq_V>=-V_eps && Vq_V<=V_eps 74 | % u(2) = 0; 75 | if Vq_V<=0 76 | if state(4)+min(a_mat)*t_step <= g.min(4) 77 | u(2) = 0; 78 | else 79 | u(2) = min(a_mat); 80 | end 81 | elseif Vq_V>=0 82 | if state(4)+max(a_mat)*t_step >= g.max(4) %|| %value <= val_threshold 83 | u(2) = 0; 84 | else 85 | u(2) = max(a_mat); 86 | end 87 | end 88 | 89 | 90 | % if value <= val_threshold 91 | % u(1:2) = 0; 92 | % end 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | --------------------------------------------------------------------------------