From 86dba69c72f8cb635daa95508ed35ae2d2b6ca1d Mon Sep 17 00:00:00 2001 From: MariusWiggert Date: Thu, 8 Jul 2021 15:55:03 +0200 Subject: [PATCH 1/7] added Masking the Hamiltonian as a way to deal with moving obstacles to HJIPDE_solve, Note: it requires a modified genericHam that uses this mask --- valFuncs/HJIPDE_solve.m | 47 +++++++++++++++++++++++++++++------------ 1 file changed, 34 insertions(+), 13 deletions(-) diff --git a/valFuncs/HJIPDE_solve.m b/valFuncs/HJIPDE_solve.m index 815c1d9..8a8e07a 100644 --- a/valFuncs/HJIPDE_solve.m +++ b/valFuncs/HJIPDE_solve.m @@ -358,11 +358,21 @@ error('Inconsistent obstacle dimensions!') end - % We always take the max between the data and the obstacles - % note that obstacles are negated. That's because if you define the - % obstacles using something like ShapeSphere, it sets it up as a - % target. To make it an obstacle we just negate that. - data0 = max(data0, -obstacle_i); + % We implement two variants of incorporating obstacles + % 1) Setting the speed of the front to 0 when it reaches the obstacle + % 2) The Reach-Avoid formulation (see Jaime's Thesis & Paper) + + + if isfield(extraArgs, 'obstacle_mask') + % 1) make obstacles to 0-1 masks (1 where no obstacle, 0 in obstacle) + obstacle_mask_i = (obstacle_i > 0); + else + % We always take the max between the data and the obstacles + % note that obstacles are negated. That's because if you define the + % obstacles using something like ShapeSphere, it sets it up as a + % target. To make it an obstacle we just negate that. + data0 = max(data0, -obstacle_i); + end end %---Extract the information about targets---------------------------------- @@ -873,11 +883,17 @@ %% Extract dynamical system if needed +% Note: Marius changed this for the case when Lagrangian is not 0! if isfield(schemeData, 'dynSys') - schemeData.hamFunc = @genericHam; +% schemeData.hamFunc = @genericHam; schemeData.partialFunc = @genericPartial; end +if ~isfield(schemeData, 'hamFunc') + schemeData.hamFunc = @genericHam; +end + +%% continue stopConverge = false; if isfield(extraArgs, 'stopConverge') stopConverge = extraArgs.stopConverge; @@ -1028,6 +1044,17 @@ if ~quiet fprintf(' Computing [%f %f]...\n', tNow, tau(i)) end + + % obstacle value function or mask if defined + if isfield(extraArgs, 'obstacleFunction') + if strcmp(obsMode, 'time-varying') + obstacle_i = obstacles(clns{:}, i); + end + if isfield(extraArgs, 'obstacle_mask') + obstacle_mask_i = (obstacle_i >0); + schemeData.obstacle_mask_i = obstacle_mask_i; + end + end % Solve hamiltonian and apply to value function (y) to get updated @@ -1166,14 +1193,8 @@ error('check your discountFactor and discountMode') end - - - - % "Mask" using obstacles + % "Mask" using obstacles (Reach-Avoid Formulation) if isfield(extraArgs, 'obstacleFunction') - if strcmp(obsMode, 'time-varying') - obstacle_i = obstacles(clns{:}, i); - end y = max(y, -obstacle_i(:)); end From 234051ac61ff4087c0c6d4a2a18f9b3f37330acf Mon Sep 17 00:00:00 2001 From: MariusWiggert Date: Fri, 16 Jul 2021 14:06:31 +0200 Subject: [PATCH 2/7] Update README.md --- README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/README.md b/README.md index 6e36ce1..a1d8ecf 100644 --- a/README.md +++ b/README.md @@ -6,3 +6,10 @@ helperOC requires MATLAB, and depends on the Level Set Methods Toolbox by Ian Mi ## Quick-start guide To begin using the repository, please refer to the "Introduction to Reachability Code" pdf in the repo + +## Related Projects +### C++ +* [BEACLS](https://hjreachability.github.io/beacls/) +### Python using various forms for speed-up +* [hj_reachability](https://github.com/StanfordASL/hj_reachability) +* [optimized_dp](https://github.com/SFU-MARS/optimized_dp) From 25f5f043ca08e5ce509f804c7724e34a4d4106bb Mon Sep 17 00:00:00 2001 From: MariusWiggert Date: Fri, 16 Jul 2021 14:08:31 +0200 Subject: [PATCH 3/7] Update README.md --- README.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index a1d8ecf..0dd5bf8 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,15 @@ # helperOC -helperOC is an optimal control toolbox for Hamilton-Jacobi Reachability Analysis. All code in this repository is written in MATLAB. There is a corresponding C++ version "Berkeley Efficient API in C++ for Level Set methods" (BEACLS) here: https://hjreachability.github.io/beacls/ +helperOC is an optimal control toolbox for Hamilton-Jacobi Reachability Analysis. All code in this repository is written in MATLAB. There is a corresponding C++ version "Berkeley Efficient API in C++ for Level Set methods" (BEACLS) here: https://hjreachability.github.io/beacls/ ## Dependencies -helperOC requires MATLAB, and depends on the Level Set Methods Toolbox by Ian Mitchell (https://bitbucket.org/ian_mitchell/toolboxls). +helperOC requires MATLAB, and depends on the Level Set Methods Toolbox by Ian Mitchell [A Toolbox of Level Set Methods ](https://www.cs.ubc.ca/~mitchell/ToolboxLS/). ## Quick-start guide -To begin using the repository, please refer to the "Introduction to Reachability Code" pdf in the repo +To begin using the repository, please refer to the "Introduction to Reachability Code" pdf in the repo and the tutorial.m file. ## Related Projects +### MATLAB +* [A Toolbox of Level Set Methods ](https://www.cs.ubc.ca/~mitchell/ToolboxLS/) ### C++ * [BEACLS](https://hjreachability.github.io/beacls/) ### Python using various forms for speed-up From a4ad95d1213f5943fdf66cf879c51de2bc30374a Mon Sep 17 00:00:00 2001 From: MariusWiggert Date: Fri, 16 Jul 2021 14:17:06 +0200 Subject: [PATCH 4/7] Implemented a different way to deal with (potentially moving) obstacles, via setting the Hamiltonian to zero at those grid points. --- .gitignore | 1 + Hamiltonians/genericHam.m | 6 ++++++ valFuncs/HJIPDE_solve.m | 7 +------ 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index de10ee6..68e0604 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ *.asv *.m~ +*.DS_Store diff --git a/Hamiltonians/genericHam.m b/Hamiltonians/genericHam.m index 1dc3e20..dc12832 100644 --- a/Hamiltonians/genericHam.m +++ b/Hamiltonians/genericHam.m @@ -84,4 +84,10 @@ hamValue = -hamValue; end end + +%% If Obstacle mask is provided zero out certain elements +if isfield(schemeData, 'obstacle_mask_i') + hamValue = hamValue .* schemeData.obstacle_mask_i; +end + end \ No newline at end of file diff --git a/valFuncs/HJIPDE_solve.m b/valFuncs/HJIPDE_solve.m index 8a8e07a..af273e6 100644 --- a/valFuncs/HJIPDE_solve.m +++ b/valFuncs/HJIPDE_solve.m @@ -883,16 +883,11 @@ %% Extract dynamical system if needed -% Note: Marius changed this for the case when Lagrangian is not 0! if isfield(schemeData, 'dynSys') -% schemeData.hamFunc = @genericHam; + schemeData.hamFunc = @genericHam; schemeData.partialFunc = @genericPartial; end -if ~isfield(schemeData, 'hamFunc') - schemeData.hamFunc = @genericHam; -end - %% continue stopConverge = false; if isfield(extraArgs, 'stopConverge') From a13d9f4379c12df058beea670a064b178d105e87 Mon Sep 17 00:00:00 2001 From: MariusWiggert Date: Fri, 16 Jul 2021 15:09:27 +0200 Subject: [PATCH 5/7] specified that for now only works for static obstacles --- valFuncs/HJIPDE_solve.m | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/valFuncs/HJIPDE_solve.m b/valFuncs/HJIPDE_solve.m index af273e6..52770e2 100644 --- a/valFuncs/HJIPDE_solve.m +++ b/valFuncs/HJIPDE_solve.m @@ -360,12 +360,20 @@ % We implement two variants of incorporating obstacles % 1) Setting the speed of the front to 0 when it reaches the obstacle + % Note: this only works for static obstacles, time-varying is + % possible but needs modifications to the Hamiltonian. + % See paper: "Path planning in multi-scale ocean flows: Coordination + % and dynamic obstacles" % 2) The Reach-Avoid formulation (see Jaime's Thesis & Paper) if isfield(extraArgs, 'obstacle_mask') % 1) make obstacles to 0-1 masks (1 where no obstacle, 0 in obstacle) + if strcmp(obsMode, 'time-varying') + error('This obstacle method is only implemented for static obstacles') + end obstacle_mask_i = (obstacle_i > 0); + schemeData.obstacle_mask_i = obstacle_mask_i; else % We always take the max between the data and the obstacles % note that obstacles are negated. That's because if you define the @@ -1039,17 +1047,6 @@ if ~quiet fprintf(' Computing [%f %f]...\n', tNow, tau(i)) end - - % obstacle value function or mask if defined - if isfield(extraArgs, 'obstacleFunction') - if strcmp(obsMode, 'time-varying') - obstacle_i = obstacles(clns{:}, i); - end - if isfield(extraArgs, 'obstacle_mask') - obstacle_mask_i = (obstacle_i >0); - schemeData.obstacle_mask_i = obstacle_mask_i; - end - end % Solve hamiltonian and apply to value function (y) to get updated @@ -1189,11 +1186,13 @@ end % "Mask" using obstacles (Reach-Avoid Formulation) - if isfield(extraArgs, 'obstacleFunction') + if isfield(extraArgs, 'obstacleFunction') && ~isfield(extraArgs, 'obstacle_mask') + if strcmp(obsMode, 'time-varying') + obstacle_i = obstacles(clns{:}, i); + end y = max(y, -obstacle_i(:)); end - % Update target function if isfield(extraArgs, 'targetFunction') if strcmp(targMode, 'time-varying') @@ -1201,7 +1200,6 @@ end end - end % Reshape value function From e33ba737af16d58391a47df085d28a5348e4aa9b Mon Sep 17 00:00:00 2001 From: MariusWiggert Date: Fri, 16 Jul 2021 15:11:15 +0200 Subject: [PATCH 6/7] more description --- valFuncs/HJIPDE_solve.m | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/valFuncs/HJIPDE_solve.m b/valFuncs/HJIPDE_solve.m index 52770e2..931c4d0 100644 --- a/valFuncs/HJIPDE_solve.m +++ b/valFuncs/HJIPDE_solve.m @@ -363,7 +363,8 @@ % Note: this only works for static obstacles, time-varying is % possible but needs modifications to the Hamiltonian. % See paper: "Path planning in multi-scale ocean flows: Coordination - % and dynamic obstacles" + % and dynamic obstacles". Also doesn't work with adversarial + % disturbance. % 2) The Reach-Avoid formulation (see Jaime's Thesis & Paper) From e33290af8085a3b9dc6a5dc1af0d6a8684c5a7b5 Mon Sep 17 00:00:00 2001 From: MariusWiggert Date: Fri, 16 Jul 2021 16:17:56 +0200 Subject: [PATCH 7/7] Added possibility to specify a running cost in the dynamical system --- Hamiltonians/genericHam.m | 5 +++++ dynSys/@DynSys/DynSys.m | 3 +++ valFuncs/HJIPDE_solve.m | 4 ++++ 3 files changed, 12 insertions(+) diff --git a/Hamiltonians/genericHam.m b/Hamiltonians/genericHam.m index dc12832..5a3b7f5 100644 --- a/Hamiltonians/genericHam.m +++ b/Hamiltonians/genericHam.m @@ -74,6 +74,11 @@ hamValue = hamValue + TIderiv*TIdx{1}; end +%% Optional: add the running cost term if present +if schemeData.dynSys.runningCost + hamValue = hamValue + dynSys.runningCostfunc(t, schemeData.grid.xs, u, d); +end + %% Negate hamValue if backward reachable set if strcmp(schemeData.tMode, 'backward') hamValue = -hamValue; diff --git a/dynSys/@DynSys/DynSys.m b/dynSys/@DynSys/DynSys.m index 40ef7a4..e332b05 100644 --- a/dynSys/@DynSys/DynSys.m +++ b/dynSys/@DynSys/DynSys.m @@ -29,6 +29,9 @@ % Data (any data that one may want to store for convenience) data + + % Add a bool if a running cost is used. False per default. + runningCost = 0; end % end properties % No constructor in DynSys class. Use constructors in the subclasses diff --git a/valFuncs/HJIPDE_solve.m b/valFuncs/HJIPDE_solve.m index 931c4d0..8e1cdd2 100644 --- a/valFuncs/HJIPDE_solve.m +++ b/valFuncs/HJIPDE_solve.m @@ -218,6 +218,10 @@ gDim = g.dim; clns = repmat({':'}, 1, gDim); +%% Give a warning when running cost is used +if schemeData.dynSys.runningCost + warning('Beware: with a running cost, the zero-level set has not the reachable set meaning anymore. Make sure that optCtrl and optDstb are subset with running cost.'); +end %% Backwards compatible if isfield(extraArgs, 'low_memory')