Implemented a second way of dealing with static obstacles & readme update & runningCost possibility

.gitignore

@@ -1,3 +1,4 @@
 *.asv
 
 *.m~
+*.DS_Store

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

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)

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

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,26 +1190,21 @@
             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')
                 target_i = targets(clns{:}, i);
             end
         end
 
-
     end
 
     % Reshape value function