ref: models

AStar/RUN_Astar.m

@@ -0,0 +1,63 @@
+% A*: Single Robot Path Planning Algorithm - MATLAB
+% Main code for running the algorithm.
+% Morteza Haghbeigi, [email protected]
+
+% Initialization
+clc
+clear
+close
+
+% adding paths
+addpath('..\models');
+addpath('..\common');
+
+%% settings
+Model.expandMethod = 'heading'; % random or heading
+Model.distType = 'manhattan'; % euclidean or manhattan;
+Model.adjType = '8adj'; % 4adj or 8adj
+
+%% create Map and Model - loading a Map Matrix
+
+% % load Map and create model - (1:free, o:obstacles)
+%  load(map_name, 'Map');
+% Model = createModelFromMap(Map, Model);
+% % add robot data to model
+% Model = addRobotToModel(Model);
+
+% % Create Model from Samples
+% Model = createModelSamples("Obstacle1", Model);
+
+% Create Map and Model by User
+Model = createModelBase(Model);
+
+% complete base model for Astar
+Model = createModelAstar(Model);
+
+%% optimal path by Astar
+% Path: nodeNumbers, coords, dirs
+tic
+[Model, Path] = myAStar(Model);
+Sol = Path;
+Sol.runTime = toc;
+Sol.cost = costL(Sol.coords);
+Sol.smoothness = smoothness_by_dir(Sol);
+
+%% modify path
+tic
+Mpath = modifyPath (Model, Path);
+Msol = Mpath;
+Msol.runTime = Sol.runTime + toc;
+Msol.cost = costL(Msol.coords);
+Msol.smoothness = smoothness(Msol.coords);
+%Msol.smoothness = smoothness_by_dir(Msol);  #todo
+
+%% # display data and plot solution
+disp(['run time for path= ' num2str(Sol.runTime)])
+disp(Sol)
+
+plotModel(Model)
+plotSolution(Sol.coords, Msol.coords)
+% plotAnimation2(Sol.coords)
+
+%% clear temporal data
+clear adj_type dist_type

AStar/RUN_Astar_Replanning.m

@@ -0,0 +1,102 @@
+% A* replanning: Single Robot Path Planning Algorithm - MATLAB
+% with remapping in case of new obstacle detection
+% Main code for running the algorithm.
+% Morteza Haghbeigi, [email protected]
+
+% Initialization
+clc
+clear
+close
+
+% adding paths
+addpath('..\models');
+addpath('..\common');
+
+%% setting
+Model.expandMethod = 'random';  % random or heading
+Model.distType = 'manhattan';    % euclidean or manhattan;
+Model.adjType = '4adj';          % 4adj or 8adj
+
+%% create Map and Model - loading a Map Matrix
+
+% % load Map and create model - (1:free, o:obstacles)
+%  load(map_name, 'Map');
+% Model = createModelFromMap(Map, Model);
+
+% % add robot data to model
+% Model = addRobotToModel(Model);
+
+% % Create Model from Samples
+% Model = createModelSamples("Obstacle1", Model);
+
+% Create Map and Model by User
+Model = createModelBase(Model);
+
+Model_init = Model;
+
+%% start timer
+tic
+
+%% pp
+% initial pp t0 (each displacement btween nodes takes 1 sec)
+[Model, Path] = myAStar(Model);
+
+% preallocation
+newObstNode=0;
+Sol.nodeNumbers = [0];
+
+t=0;
+pt=0;
+while Sol.nodeNumbers(end)~=Model.Robot.targetNode
+    t=t+1;
+    pt=pt+1;
+
+    % update model (insert new obstacles)
+    if t==5
+        newObstNode(end+1) = 36;
+        Model.Obsts.x(end+1) = 4;
+        Model.Obsts.y(end+1) = 0;
+        Model.Obsts.nodeNumber(end+1) = 37;
+        Model.Obsts.count = Model.Obsts.count+1;
+    end
+    % check if path replanning is needed
+    if any(Path.nodeNumbers(pt) == Model.Obsts.nodeNumber)
+        Model.Robot.startNode = Sol.nodeNumbers(end);
+        xy = Model.Nodes.cord(:, Model.Robot.startNode);
+        Model.xs = xy(1);
+        Model.ys = xy(2);
+        dirs = nodes2dirs(Sol.nodeNumbers, Model);
+        Model.dir = dirs(end);
+        [Model, Path] = myAStar(Model);
+        pt=2;
+    end
+
+    % update final sol
+    Sol.nodeNumbers(t) = Path.nodeNumbers(pt);
+end
+
+% process time
+Sol.runTime = toc;
+
+% final solution
+Sol.coords = nodes2coords(Sol.nodeNumbers, Model);
+Sol.dirs = nodes2dirs(Sol.nodeNumbers, Model);
+
+%% update model and calculate cost
+newObstNode = newObstNode(2:end);
+if numel(newObstNode)>0
+    Model_init.xc = [Model.Obsts.x Model.Nodes.cord(1,newObstNode)];
+    Model_init.yc = [Model.Obsts.y Model.Nodes.cord(2,newObstNode)];
+end
+Sol.cost = costL(Sol.coords);
+Sol.smoothness = smoothness_by_dir(Sol);
+
+%% display data and plot solution
+disp(Sol)
+
+plotModel(Model)
+plotSolution(Sol.coords,[])
+% plotAnimation2(Sol.coords)
+
+%% clear temporal data
+clear xy t pt dirs newobstNode adj_type dist_type

AStar/createModelAstar.m

@@ -0,0 +1,81 @@
+function Model=createModelAstar(Model)
+% add neccessary data for Astar to Base Model
+
+disp('Complete Model for AStar');
+
+%% 
+Nodes = Model.Nodes;
+
+switch Model.adjType
+    case '4adj'
+        ixy = [1 0; 0 1; 0 -1; -1 0];
+        nAdj=4;
+    case '8adj'
+        ixy = [1 0; 0 1; 0 -1; -1 0; 1 1; -1 -1; 1 -1; -1 1];
+        nAdj=8;
+end
+
+% euclidean manhattan
+switch Model.distType
+    case 'manhattan'
+        edgeLength=2;
+    case 'euclidean'
+        edgeLength=sqrt(2);
+end
+
+% tempNeighbor
+tempNeighbor.nodeNumber = 0;
+tempNeighbor.cost = 0;
+tempNeighbor.dir = 0;
+tempNeighbor.x = 0;
+tempNeighbor.y = 0;
+tempNodesNeigh.count = 0;
+tempNodesNeigh.List = repmat(tempNeighbor, nAdj, 1);
+
+% Neighbors
+nNodes = Model.Nodes.count;
+Neighbors = repmat(tempNodesNeigh, nNodes, 1);
+
+for iNode=1:nNodes
+    Neighbors(iNode).count = 0;
+    if ~any(iNode==Model.Obsts.nodeNumber)
+        xNode = Nodes.cord(1,iNode);
+        yNode = Nodes.cord(2,iNode);
+        for iAdj=1:nAdj
+            ix=ixy(iAdj,1);
+            iy=ixy(iAdj,2);
+            newX = xNode+ix;
+            newY = yNode+iy;
+            newDir = atan2(iy, ix);
+
+            % check if the Node is within array bound
+            if(newX>=Model.Map.xMin && newX<=Model.Map.xMax) && (newY>=Model.Map.yMin && newY<=Model.Map.yMax)
+                newNodeNumber = iNode+ix+iy*(Model.Map.nX);
+
+                if ~any(newNodeNumber==Model.Obsts.nodeNumber)
+                    if ix~=0 && iy~=0
+                        cost = edgeLength;
+                    else
+                        cost = 1;
+                    end
+                    newNeighbor = tempNeighbor;
+                    newNeighbor.nodeNumber = newNodeNumber;
+                    newNeighbor.dir = newDir;
+                    newNeighbor.cost = cost;
+                    newNeighbor.x = newX;
+                    newNeighbor.y = newY;
+                    Neighbors(iNode).count = Neighbors(iNode).count + 1;
+                    Neighbors(iNode).List(Neighbors(iNode).count) = newNeighbor;
+                end
+            end
+        end
+    end
+end
+
+%% save model
+Model.Neighbors=Neighbors;
+
+%% plot model
+% plotModel(model);
+
+end

AStar/expand.m

@@ -0,0 +1,40 @@
+function expand = expand(TopNode, Closed, Model)
+    % arrange neighbors based on cost robot and direction
+    % nodeNumber pNode gCost fCost dir
+
+    nNeighbors = Model.Neighbors(TopNode.nodeNumber).count;
+    Neighbors = Model.Neighbors(TopNode.nodeNumber).List;
+
+    nExpand = 0;
+    for iNeighbor = 1:nNeighbors
+        % neighbot
+        newX = Neighbors(iNeighbor).x;
+        newY = Neighbors(iNeighbor).y;
+        newDir = Neighbors(iNeighbor).dir;
+        newCost = Neighbors(iNeighbor).cost;
+        newNodeNumber = Neighbors(iNeighbor).nodeNumber;
+
+        % add newNode to list if it is not in Closed list
+        if ~any(newNodeNumber == Closed.nodeNumber)
+            nExpand = nExpand + 1;
+            list(nExpand).visited = 0;
+            list(nExpand).nodeNumber = newNodeNumber;
+            list(nExpand).pNode = TopNode.nodeNumber;
+            list(nExpand).gCost = TopNode.gCost + newCost;
+            hCost = Distance(Model.Robot.xt, Model.Robot.yt, newX, newY, Model.distType);
+            list(nExpand).fCost = list(nExpand).gCost + hCost;
+            list(nExpand).dir = newDir;
+        end
+
+    end
+
+    % Neighbors
+    expand.count = nExpand;
+
+    if nExpand ~= 0
+        expand.List = list;
+    else
+        expand.List = [];
+    end
+
+end

AStar/initializeAstar.m

@@ -0,0 +1,30 @@
+function [Closed, Open, TopNode] = initializeAstar(Model)
+% Initialize Astar PP problem.
+
+
+% Closed: count, nodeNumber
+% Closed: put all obstacles on the Closed list
+Closed.count = Model.Obsts.count;
+Closed.nodeNumber = Model.Obsts.nodeNumber;
+
+% Open: count, List
+% Open.List Fields: visited nodeNumber pNode gCost fCost dir
+
+% set the starting node as the first node in Open (TopNode)
+TopNode.visited = 1;
+TopNode.nodeNumber=Model.Robot.startNode;
+TopNode.pNode=Model.Robot.startNode;
+TopNode.dir = Model.Robot.dir;
+TopNode.gCost = 0;
+hCost = Distance(Model.Robot.xs, Model.Robot.ys,  Model.Robot.xt,  Model.Robot.yt, Model.distType);
+TopNode.fCost = TopNode.gCost + hCost;
+
+% insert TopNode (start node) in Open list
+Open.count = 1;
+Open.List(1) = TopNode;
+
+% add last node to Closed
+Closed.count = Closed.count+1;
+Closed.nodeNumber(Closed.count) = TopNode.nodeNumber;
+
+end

AStar/myAStar.m

@@ -0,0 +1,33 @@
+function [Model, Path] = myAStar(Model)
+    % Astar algorithm
+
+    % initialization
+    [Closed, Open, TopNode] = initializeAstar(Model);
+
+    %%%% Start Algorithm
+
+    while TopNode.nodeNumber ~= Model.Robot.targetNode
+
+        % finding neighbors (successors)
+        Neighbors = expand(TopNode, Closed, Model);
+
+        % switch Model.adjType
+        %     case  '4adj'
+        %         Neighbors=neighbors4(TopNode, Closed, Model);
+        %     case '8adj'
+        %         Neighbors=neighbors8(TopNode, Closed, Model);
+        % end
+
+        % update or extend Open list with the successor nodes
+        Open = updateOpen(Open, Neighbors);
+
+        % select new Top Node
+        [TopNode, Open] = selectTopNode(Open, TopNode, Model.expandMethod, Model.Robot.targetNode);
+        Closed.count = Closed.count + 1;
+        Closed.nodeNumber(Closed.count) = TopNode.nodeNumber;
+    end
+
+    % optimal paths coordinations, nodes, directions
+    Path = optimalPath(Model, Open);
+
+end