From a1bc1611e988649ce37b5e51ab07742955461804 Mon Sep 17 00:00:00 2001 From: Michael Lauria Date: Sun, 8 Aug 2021 20:50:46 -0600 Subject: [PATCH 1/3] add mountaincar-v0 --- dynSys/@MountainCarV0/MountainCarV0.m | 33 +++++ dynSys/@MountainCarV0/dynamics.m | 30 +++++ dynSys/@MountainCarV0/optCtrl.m | 43 +++++++ mountain_car.m | 179 ++++++++++++++++++++++++++ 4 files changed, 285 insertions(+) create mode 100644 dynSys/@MountainCarV0/MountainCarV0.m create mode 100644 dynSys/@MountainCarV0/dynamics.m create mode 100644 dynSys/@MountainCarV0/optCtrl.m create mode 100644 mountain_car.m diff --git a/dynSys/@MountainCarV0/MountainCarV0.m b/dynSys/@MountainCarV0/MountainCarV0.m new file mode 100644 index 0000000..684eafc --- /dev/null +++ b/dynSys/@MountainCarV0/MountainCarV0.m @@ -0,0 +1,33 @@ +classdef MountainCarV0 < DynSys + properties + gravity + force + end + + methods + function obj = MountainCarV0(x, gravity, force) + + if numel(x) ~= 2 + error('Initial state does not have right dimension!'); + end + + if ~iscolumn(x) + x = x'; + end + + + obj.x = x; + obj.xhist = obj.x; + + obj.pdim = 1; % position dimensions + + obj.nx = 2; % number of state dimensions + obj.nu = 1; % number of control dimensions + + obj.gravity = gravity; % gravity (default is 0.0025) + obj.force = force; % force (default is 0.001) + + end + + end % end methods +end % end classdef diff --git a/dynSys/@MountainCarV0/dynamics.m b/dynSys/@MountainCarV0/dynamics.m new file mode 100644 index 0000000..4a1dfd0 --- /dev/null +++ b/dynSys/@MountainCarV0/dynamics.m @@ -0,0 +1,30 @@ +function dx = dynamics(obj, t, x, u, d) + + gravity = obj.gravity; + force = obj.force; + +% velocity += (action - 1) * self.force + math.cos(3 * position) * (-self.gravity) +% velocity = np.clip(velocity, -self.max_speed, self.max_speed) +% position += velocity +% position = np.clip(position, self.min_position, self.max_position) +if iscell(x) + dx = cell(obj.nx, 1); + + position = x{1}; + velocity = x{2}; + + dx{1} = velocity; + dx{2} = (u - 1) * force + cos(3 * position) * (-gravity); +else + dx = zeros(obj.nx, 1); + + position = x(1); + velocity = x(2); + + dx(1) = velocity; + % u ~ { 0, 1, 2 } + dx(2) = (u - 1) * force + cos(3 * position) * (-gravity); +end + + +end \ No newline at end of file diff --git a/dynSys/@MountainCarV0/optCtrl.m b/dynSys/@MountainCarV0/optCtrl.m new file mode 100644 index 0000000..2fdad3c --- /dev/null +++ b/dynSys/@MountainCarV0/optCtrl.m @@ -0,0 +1,43 @@ +function uOpt = optCtrl(obj, t, xs, deriv, uMode, ~) +% uOpt = optCtrl(obj, t, deriv, uMode, dMode, MIEdims) + +position = xs{1}; +velocity = xs{2}; + +%% Input processing +if nargin < 5 + uMode = 'min'; +end + + +%% https://github.com/ZhiqingXiao/OpenAIGymSolution/blob/master/MountainCar-v0_close_form/mountaincar_v0_close_form.ipynb +% position, velocity = observation +% lb = min(-0.09 * (position + 0.25) ** 2 + 0.03, +% 0.3 * (position + 0.9) ** 4 - 0.008) +% ub = -0.07 * (position + 0.38) ** 2 + 0.07 +% if lb < velocity < ub: +% action = 2 # push right +% else: +% action = 0 # push left + +% TODO: Use deriv instead of hard-coded values above? +% TODO: not sure this is working... + + +lb = min(-0.09 * (position + 0.25)^2 + 0.03, ... + 0.3 * (position + 0.9)^4 - 0.008); + +ub = -0.07 * (position + 0.38)^2 + 0.07; + +% action = 0; % push left +% if (lb < velocity) & (velocity < ub) +% action = 2; % push right +% end +% TODO: handle uMode (always min for Backwards Reachability with goal) + +%% Optimal control +actions = zeros(141); +actions((lb < velocity) & (velocity < ub)) = 2; +uOpt = actions; + +end \ No newline at end of file diff --git a/mountain_car.m b/mountain_car.m new file mode 100644 index 0000000..99e7bba --- /dev/null +++ b/mountain_car.m @@ -0,0 +1,179 @@ +%% https://rvl.cs.toronto.edu/backwards-reachability/#22-in-two-simple-dimensions +%% https://github.com/rvl-lab-utoronto/backwards-reachability + +%% https://www.cs.ubc.ca/~mitchell/ToolboxLS/ +%% https://github.com/HJReachability/helperOC/blob/master/Intro%20to%20Reachability%20Code.pdf + + +%% https://gieseanw.wordpress.com/2012/10/21/a-comprehensive-step-by-step-tutorial-to-computing-dubins-paths/ + +function mountain_car() +% 1. Run Backward Reachable Set (BRS) with a goal +% uMode = 'min' <-- goal +% minWith = 'none' <-- Set (not tube) +% compTraj = false <-- no trajectory +% 2. Run BRS with goal, then optimal trajectory +% uMode = 'min' <-- goal +% minWith = 'none' <-- Set (not tube) +% compTraj = true <-- compute optimal trajectory +% 3. Run Backward Reachable Tube (BRT) with a goal, then optimal trajectory +% uMode = 'min' <-- goal +% minWith = 'minVOverTime' <-- Tube (not set) +% compTraj = true <-- compute optimal trajectory +% 5. Change to an avoid BRT rather than a goal BRT +% uMode = 'max' <-- avoid +% dMode = 'min' <-- opposite of uMode +% minWith = 'minVOverTime' <-- Tube (not set) +% compTraj = false <-- no trajectory +% 6. Change to a Forward Reachable Tube (FRT) +% add schemeData.tMode = 'forward' +% note: now having uMode = 'max' essentially says "see how far I can +% reach" +% 7. Add obstacles +% add the following code: +% obstacles = shapeCylinder(g, 3, [-1.5; 1.5; 0], 0.75); +% HJIextraArgs.obstacles = obstacles; +% 8. Add random disturbance (white noise) +% add the following code: +% HJIextraArgs.addGaussianNoiseStandardDeviation = [0; 0; 0.5]; + + +%% https://en.wikipedia.org/wiki/Mountain_car_problem +%% https://github.com/openai/gym/blob/master/gym/envs/classic_control/mountain_car.py + +min_velocity = -0.07; +max_velocity = 0.07; + +min_position = -1.2; +max_position = 0.6; + +goal_position = 0.5; +goal_velocity = 0.0; + +%% Should we compute the trajectory? +% compTraj = true; +compTraj = false; + +%% Grid +grid_min = [min_position; min_velocity]; % Lower corner of computation domain +grid_max = [max_position; max_velocity]; % Upper corner of computation domain +N = [141; 141]; % Number of grid points per dimension +grid = createGrid(grid_min, grid_max, N); + +% state space dimensions + +%% target set +% R = 1; +% data0 = shapeCylinder(grid,ignoreDims,center,radius) +% making the inital shape +% data0 = shapeCylinder(grid, 3, [0; 0; 0], R); +% also try shapeRectangleByCorners, shapeSphere, etc. + +center = [0.55; 0]; +widths = [0.05; 0.01]; + + +center = [0.55; 0]; +widths = [0.15; 0.03]; + +data0 = shapeRectangleByCenter(grid, center, widths); + +%% time vector +t0 = 0; +% changed the time from 2 seconds to 15 +tMax = 20; +dt = 0.1; +tau = t0:dt:tMax; + +%% problem parameters +gravity = 0.0025; +force = 0.001; + +% control trying to min or max value function? +uMode = 'min'; + + +%% Pack problem parameters + +% Define dynamic system +mCar = MountainCarV0([0, 0], gravity, force); + +% Put grid and dynamic systems into schemeData +schemeData.grid = grid; +schemeData.dynSys = mCar; +% schemeData.accuracy = 'high'; +schemeData.accuracy = 'low'; +schemeData.uMode = uMode; + +%% Compute value function + +HJIextraArgs.visualize.valueSet = 1; +HJIextraArgs.visualize.initialValueSet = 1; +HJIextraArgs.visualize.figNum = 1; %set figure number +HJIextraArgs.visualize.deleteLastPlot = true; %delete previous plot as you update +% HJIextraArgs.visualize.valueSetHeatMap = true; + +HJIextraArgs.makeVideo = true; % generate video of output +% You can further customize the video file in the following ways +% HJIextraArgs.videoFilename: (string) filename of video +% HJIextraArgs.frameRate: (int) framerate of video +HJIextraArgs.visualize.xTitle = 'x axis'; +HJIextraArgs.visualize.yTitle = 'y axis'; + +% uncomment if you want to see a 2D slice +% HJIextraArgs.visualize.plotData.plotDims = [1 1 0]; %plot x, y +% HJIextraArgs.visualize.plotData.projpt = [0]; %project at theta = 0 +% HJIextraArgs.visualize.viewAngle = [0,90]; % view 2D + +%[data, tau, extraOuts] = ... +% HJIPDE_solve(data0, tau, schemeData, minWith, extraArgs) + +minWith = 'none'; +% minWith = 'maxVOverTime'; + +[data, tau2, ~] = ... + HJIPDE_solve(data0, tau, schemeData, minWith, HJIextraArgs); + +%% Compute optimal trajectory from some initial state +if compTraj + pause + + %set the initial state + xinit = [2, 2]; + + figure(6) + clf + h = visSetIm(g, data(:,:,:,end)); + h.FaceAlpha = .3; + hold on + s = scatter3(xinit(1), xinit(2), xinit(3)); + s.SizeData = 70; + + %check if this initial state is in the BRS/BRT + %value = eval_u(g, data, x) + value = eval_u(g,data(:,:,:,end),xinit); + + if value <= 0 %if initial state is in BRS/BRT + % find optimal trajectory + + mCar.x = xinit; %set initial state of the dubins car + + TrajextraArgs.uMode = uMode; %set if control wants to min or max + TrajextraArgs.visualize = true; %show plot + TrajextraArgs.fig_num = 2; %figure number + + %we want to see the first two dimensions (x and y) + TrajextraArgs.projDim = [1 1 0]; + + %flip data time points so we start from the beginning of time + dataTraj = flip(data,4); + + % [traj, traj_tau] = ... + % computeOptTraj(g, data, tau, dynSys, extraArgs) + [traj, traj_tau] = ... + computeOptTraj(g, dataTraj, tau2, mCar, TrajextraArgs); + else + error(['Initial state is not in the BRS/BRT! It have a value of ' num2str(value,2)]) + end +end +end \ No newline at end of file From af20dc690bcc172900d58666f10a79ad54fdd822 Mon Sep 17 00:00:00 2001 From: Michael Lauria Date: Sun, 8 Aug 2021 22:05:49 -0600 Subject: [PATCH 2/3] fix magic number --- dynSys/@MountainCarV0/optCtrl.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dynSys/@MountainCarV0/optCtrl.m b/dynSys/@MountainCarV0/optCtrl.m index 2fdad3c..a358914 100644 --- a/dynSys/@MountainCarV0/optCtrl.m +++ b/dynSys/@MountainCarV0/optCtrl.m @@ -36,7 +36,7 @@ % TODO: handle uMode (always min for Backwards Reachability with goal) %% Optimal control -actions = zeros(141); +actions = zeros(size(xs{1})); actions((lb < velocity) & (velocity < ub)) = 2; uOpt = actions; From e6b3e08b129fd8e1dedd77fa33cbec9d4efa9eae Mon Sep 17 00:00:00 2001 From: Michael Lauria Date: Mon, 9 Aug 2021 14:41:08 -0600 Subject: [PATCH 3/3] fix optCtrl using tutorial in paper appendix --- dynSys/@MountainCarV0/optCtrl.m | 48 +++++++++++++++------------------ mountain_car.m | 32 ++++++++++------------ 2 files changed, 36 insertions(+), 44 deletions(-) diff --git a/dynSys/@MountainCarV0/optCtrl.m b/dynSys/@MountainCarV0/optCtrl.m index a358914..b103a19 100644 --- a/dynSys/@MountainCarV0/optCtrl.m +++ b/dynSys/@MountainCarV0/optCtrl.m @@ -1,43 +1,39 @@ function uOpt = optCtrl(obj, t, xs, deriv, uMode, ~) % uOpt = optCtrl(obj, t, deriv, uMode, dMode, MIEdims) -position = xs{1}; -velocity = xs{2}; - %% Input processing if nargin < 5 uMode = 'min'; end +if ~iscell(deriv) + deriv = num2cell(deriv); +end -%% https://github.com/ZhiqingXiao/OpenAIGymSolution/blob/master/MountainCar-v0_close_form/mountaincar_v0_close_form.ipynb -% position, velocity = observation -% lb = min(-0.09 * (position + 0.25) ** 2 + 0.03, -% 0.3 * (position + 0.9) ** 4 - 0.008) -% ub = -0.07 * (position + 0.38) ** 2 + 0.07 -% if lb < velocity < ub: -% action = 2 # push right -% else: -% action = 0 # push left - -% TODO: Use deriv instead of hard-coded values above? -% TODO: not sure this is working... +%% Optimal control -lb = min(-0.09 * (position + 0.25)^2 + 0.03, ... - 0.3 * (position + 0.9)^4 - 0.008); +% https://arxiv.org/pdf/1709.07523.pdf +% Appendix +% need dot product of deriv and dx = f(x) +% then argmin terms with action to find optimal control -ub = -0.07 * (position + 0.38)^2 + 0.07; +% we have +% dx(1) = velocity; +% u ~ { 0, 1, 2 } +% dx(2) = (u - 1) * force + cos(3 * position) * (-gravity); -% action = 0; % push left -% if (lb < velocity) & (velocity < ub) -% action = 2; % push right -% end -% TODO: handle uMode (always min for Backwards Reachability with goal) +% u* = argmin_u (deriv{[0, 1]} * dx(2)) +% = argmin_u (deriv{[0, 1]} (u - 1) * force) +% = 0 when deriv{[0, 1]} >= 0, 2 when deriv{[0, 1]} < 0 %% Optimal control -actions = zeros(size(xs{1})); -actions((lb < velocity) & (velocity < ub)) = 2; -uOpt = actions; +if strcmp(uMode, 'max') + uOpt = (deriv{1} >= 0) * 2 + (deriv{1} < 0) * 0; +elseif strcmp(uMode, 'min') + uOpt = (deriv{1} >= 0) * 0 + (deriv{1} < 0) * 2; +else + error('Unknown uMode!') +end end \ No newline at end of file diff --git a/mountain_car.m b/mountain_car.m index 99e7bba..c42bac3 100644 --- a/mountain_car.m +++ b/mountain_car.m @@ -47,17 +47,14 @@ function mountain_car() min_position = -1.2; max_position = 0.6; -goal_position = 0.5; -goal_velocity = 0.0; - %% Should we compute the trajectory? -% compTraj = true; -compTraj = false; +compTraj = true; +% compTraj = false; %% Grid grid_min = [min_position; min_velocity]; % Lower corner of computation domain grid_max = [max_position; max_velocity]; % Upper corner of computation domain -N = [141; 141]; % Number of grid points per dimension +N = [181; 181]; % Number of grid points per dimension grid = createGrid(grid_min, grid_max, N); % state space dimensions @@ -69,19 +66,18 @@ function mountain_car() % data0 = shapeCylinder(grid, 3, [0; 0; 0], R); % also try shapeRectangleByCorners, shapeSphere, etc. -center = [0.55; 0]; -widths = [0.05; 0.01]; - +% center = [0.55; 0]; +% widths = [0.05; 0.01]; -center = [0.55; 0]; -widths = [0.15; 0.03]; +center = [0.5; 0]; +widths = [0.1; 0.02]; data0 = shapeRectangleByCenter(grid, center, widths); %% time vector t0 = 0; % changed the time from 2 seconds to 15 -tMax = 20; +tMax = 80; dt = 0.1; tau = t0:dt:tMax; @@ -102,7 +98,7 @@ function mountain_car() schemeData.grid = grid; schemeData.dynSys = mCar; % schemeData.accuracy = 'high'; -schemeData.accuracy = 'low'; +schemeData.accuracy = 'veryHigh'; schemeData.uMode = uMode; %% Compute value function @@ -139,19 +135,19 @@ function mountain_car() pause %set the initial state - xinit = [2, 2]; + xinit = [-0.5, 0]; figure(6) clf - h = visSetIm(g, data(:,:,:,end)); - h.FaceAlpha = .3; + h = visSetIm(grid, data(:,:,:,end)); +% h.FaceAlpha = .3; hold on s = scatter3(xinit(1), xinit(2), xinit(3)); s.SizeData = 70; %check if this initial state is in the BRS/BRT %value = eval_u(g, data, x) - value = eval_u(g,data(:,:,:,end),xinit); + value = eval_u(grid,data(:,:,:,end),xinit); if value <= 0 %if initial state is in BRS/BRT % find optimal trajectory @@ -171,7 +167,7 @@ function mountain_car() % [traj, traj_tau] = ... % computeOptTraj(g, data, tau, dynSys, extraArgs) [traj, traj_tau] = ... - computeOptTraj(g, dataTraj, tau2, mCar, TrajextraArgs); + computeOptTraj(grid, dataTraj, tau2, mCar, TrajextraArgs); else error(['Initial state is not in the BRS/BRT! It have a value of ' num2str(value,2)]) end