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..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; @@ -84,4 +89,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/README.md b/README.md index 6e36ce1..0dd5bf8 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,17 @@ # 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 +* [hj_reachability](https://github.com/StanfordASL/hj_reachability) +* [optimized_dp](https://github.com/SFU-MARS/optimized_dp) 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 815c1d9..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') @@ -358,11 +362,30 @@ 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 + % 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". Also doesn't work with adversarial + % disturbance. + % 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 + % 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---------------------------------- @@ -878,6 +901,7 @@ schemeData.partialFunc = @genericPartial; end +%% continue stopConverge = false; if isfield(extraArgs, 'stopConverge') stopConverge = extraArgs.stopConverge; @@ -1166,18 +1190,14 @@ error('check your discountFactor and discountMode') end - - - - % "Mask" using obstacles - if isfield(extraArgs, 'obstacleFunction') + % "Mask" using obstacles (Reach-Avoid Formulation) + 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') @@ -1185,7 +1205,6 @@ end end - end % Reshape value function