ROBOT LOCALIZATION AND MOTION PLANNING

The state of the robot is fully described by its position and orientation xk=[xk,yk,ϕk]T , expressed in the global coordinate frame marked with x and y . Given a control input uk=[rk,Δϕk]T , the robots first turns on the spot by an angle Δϕk and then moves in a straight line for a distance of rk in the direction it is facing.

Please implement the motion model as an anonymous function assigned to f that models this behaviour. This motion model will be employed to arrive at a prior estimate x^ k of the robot pose given a posterior estimate of the pose at a previous time instance xk−1 and control inputs uk as x^k=f(xk−1,uk). (x^k is estimate of xk). Please also provide the Jacobians with respect to the state xk−1 and the input uk as anonymous functions and assign them to Fx and Fu respectively.

1 f = @(x, u) ????;

2 Fx = @(x,u) ????;

3 Fu = @(x,u) ????;

[Walter Roberson]

What is the MATLAB question here? This seems to be a doit4me

[Ken]

• Thanks Walter.
• It is to find the Jacobian Fx, Fu of f w.r.t. x and u. I tried this but get error "Undefined function 'x' for input arguments of type 'double' :
• f = @(x, u) [x(1)+u(1)*cos(x(3)+u(2));
• x(2)+u(1)*sin(x(3)+u(2)) ; x(3)+u(2)];
• Fx = @(x,u) 1;1;1;
• Fu = @(x,u) cos(x(2)+1);sin(x(2)+1);1;

[Ken]

Some added info:

x = [1.; 2. ; pi/4]
u = [.1; pi/8]

Answer 1

[Walter Roberson]

test http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_419758

[Ken]

Thanks Walter.

It would be nice if the pseudocode posted on 31-12 could be modified to implement this algorithm.

[Walter Roberson]

Sorry this was a test post that I forgot to delete.

[Walter Roberson]

Take the pseudocode and mentally place the test for eligibility after each line. Does the test refer to values that have not been defined yet? Does the test refer to changing values that have not yet been correctly assigned their current value? Does the test refer to changing values which have been just changed in a way that makes it impossible to carry out the test correctly? What are the consequences of inserting the test at that point compared to inserting it one step later?

[Ken]

Thanks Walter. First attempt:

function BF(G, Start, Goal)
   Queue = Queue_init_FIFO();
   Closed = Queue_init_FIFO();
Cost = Queue_init_FIFO()
     Queue = Queue_push_FIFO(Queue, Start);
     while ~Queue_isempty_FIFO(Queue)
       [curr, Queue] = Queue_pop_FIFO(Queue)
       if isequal(curr, Goal)
         return
       end
%1. is curr =-1 i.e. an obstacle?
If curr ~= -1
continue
end
%closed = Queue_push_FIFO(Closed, curr);
       next = Graph_expand(G, curr);
       for K = 1 : size(next, 1)
         this_next = next(K,:);
         if ~ Closed_ismember_FIFO(Closed, next)
           Queue = Queue_push_FIFO(Queue, this_next);
end
%The eligible cells to expand are in Queue, choose the least costly cells %i.e. cells closest to Start

NextSolutionMap(x,y) = min((SolutionMap(x-1,y) – Start(1)+ SolutionMap(x-1,y)…
 – Start(2)),  (SolutionMap(x+1,y) – Start(1) + SolutionMap(x+1,y) – Start(2)),  ...
(SolutionMap(x,y-1) – Start(1) + SolutionMap(x,y-1) – Start(2)),…
(SolutionMap(x,y+1) – Start(1) + SolutionMap(x,y+1) – Start(2));
  Cost = Cost_push_FIFO(Cost,[x,y]);  
SolutionMap(x,y) = Cost(x,y) %problem asks for g i.e. cost values to be stored here
end
        end  
      
Made assumptions:

1. No need of closed - if there is obstacle i.e. -1, we don't expand that cell. The prob of using a cell again after expanded is not there as it would not be minimum.

2. Confused about Next, nextsolution

[Walter Roberson]

You need to strictly pair the operations on Queue and Cost -- every time you push onto Queue you must push onto Cost, and every time you pop from Queue you must pop from Cost.

You still need Closed, to avoid having locations processed multiple times.

curr will always be a coordinate pair. You need to check what is stored in the map at that coordinate pair.

[Ken]

Thanks Walter.

Feel I am out of my league here so am trying a simpler problem i.e. to find the shortest path from start to goal using the same cost i.e. 1 for vert edge, 1 for horiz edge. Tried this but stuck at Min - goes into an endless loop. Please help.

Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) = -1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;

Start = [3,7];
Goal  = [9,6];

SolutionMap = Inf*ones(size(Map)); %store g-values in here.
G = Map;
%function BF(G, Start, Goal) 
Queue = Queue_init_FIFO(); 
Closed = Queue_init_FIFO(); 
Cost = Queue_init_FIFO() 
Queue = Queue_push_FIFO(Queue, Start); 
Closed = Closed_push_FIFO(Queue, Start); 
while ~Queue_isempty_FIFO(Queue) 
[curr, Queue] = Queue_pop_FIFO(Queue); 
if isequal(curr, Goal) 
return 
end 
1. is curr =-1 i.e. an obstacle? 
for curr ~= -1 
continue 

if ~ Closed_ismember_FIFO(Closed, next)
    next = curr;
    next=Start;
    while next ~= Goal
        if next ~=-1
    x=next(1);
    y=next(2);
end
Queue = Queue_push_FIFO(Queue, this_next); 
NextSolutionMap(x,y) = min((SolutionMap(x-1,y) - Goal (1)+ SolutionMap(x-1,y)- Goal(2)),...
(SolutionMap(x+1,y)- Goal(1) + SolutionMap(x+1,y)- Goal(2)),... 
(SolutionMap(x,y-1) - Goal(1) + SolutionMap(x,y-1) - Goal(2)),...
(SolutionMap(x,y+1) - Goal(1) + SolutionMap(x,y+1) - Goal(2)));
next=NextSolutionMap(x,y);

%Cost = Cost_push_FIFO(Cost,[x,y]); 
%SolutionMap(x,y) = Cost(x,y) %problem asks for g i.e. cost values to be stored here 
end 
    end

[Ken]

Walter, Maybe I tested your patience but please help!!

Answer 2

[Walter Roberson]

Assuming your formulae are correct,

Fx = @(x,u) [1;1;1];
Fu = @(x,u) [cos(x(2)+1);sin(x(2)+1);1];

[Walter Roberson]

x = sym('x',[3,1]);
u = sym('u',[2,1]);
F = [x(1)+u(1)*cos(x(3)+u(2));
x(2)+u(1)*sin(x(3)+u(2)) ; x(3)+u(2)];
Fx = matlabFunction( jacobian(F,x), 'Vars', {x,u})
Fu = matlabFunction( jacobian(F,u), 'Vars', {x,u})

[Ken]

Trying to do it based on your prev post but get error

"Jacobian with respect to x returns a result of incorrect size! "

f = @(x, u) [x(1)+u(1)*cos(x(3)+u(2)); x(2)+u(1)*sin(x(3)+u(2)) ; x(3)+u(2)];
Fx = @(x,u) [1-u(1)*sin(x(3)+u(2));1+u(1)*cos(x(3)+u(2));1];
Fu = @(x,u) [-sin(x(3)+1);cos(x(3)+1);1];

[Ken]

Hi Walter, when I tried your last post in MATLAB, I got a very long answer - not sure what's the problem:

 Fx= @(in1,in2)reshape([1.0,0.0,0.0,0.0,1.0,0.0,-in2(1,:).*sin(in2(2,:)+in1(3,:)),in2(1,:).*cos(in2(2,:)+in1(3,:)),1.0],[3,

[Walter Roberson]

Your x is not a single variable, it is a vector. Taking the Jacobian with respect to x is going to give you an answer with respect to each component

jacobian(F,x)
ans =
[ 1, 0, -u1*sin(u2 + x3)]
[ 0, 1,  u1*cos(u2 + x3)]
[ 0, 0,                1]

[Ken]

Thanks Walter. Still says Jacobian is incorrect size. The problem statements is: A robot, depicted here as a disc with an arrow indicating its heading, traverses a planar environment populated by a set of uniquely identifiable landmarks, visualized as red circles.

The state of the robot is fully described by its position and orientation xk=[xk,yk,ϕk]T , expressed in the global coordinate frame marked with x and y . Given a control input uk=[rk,Δϕk]T , the robots first turns on the spot by an angle Δϕk and then moves in a straight line for a distance of rk in the direction it is facing.

Please implement the motion model as an anonymous function assigned to f that models this behaviour. This motion model will be employed to arrive at a prior estimate x^k of the robot pose given a posterior estimate of the pose at a previous time instance xk−1 and control inputs uk as x^k=f(xk−1,uk) . Please also provide the Jacobians with respect to the state xk−1 and the input uk as anonymous functions and assign them to Fx and Fu respectively.

[Walter Roberson]

Generate some arrays of various sizes to return as potential Jacobians and try them, and see what size it stops complaining about the size for. Then, knowing the size it expects, we could work on getting the content right.

One variation to try:

x = sym('x',[3,1]);
u = sym('u',[2,1]);
F = [x(1)+u(1)*cos(x(3)+u(2));
x(2)+u(1)*sin(x(3)+u(2)) ; x(3)+u(2)];
Fx = matlabFunction( jacobian(F,x), 'Vars', {x.', u.'})
Fu = matlabFunction( jacobian(F,u), 'Vars', {x.', u.'})

[Ken]

• Thanks Walter. Managed to get it:

f = @(x, u) [x(1)+u(1)*cos(x(3)+u(2));
             x(2)+u(1)*sin(x(3)+u(2)) ; x(3)+u(2)];

Fx = @(x,u) [1,0,-u(1)*sin(x(3)+u(2));0,1,u(1)*cos(x(3)+u(2));0,0,1];

Fu = @(x,u) [cos(x(3)+u(2)),-u(1)*sin(x(3)+u(2));...
             sin(x(3)+u(2)),u(1)*cos(x(3)+u(2));0,1];

[Ken]

I am now trying to modify your code to get from SearchStart @ [3,7] to SearchGoal@ [9,6] by moving only vertically and horizontally. Any thoughts appreciated - Thanks

[Walter Roberson]

After you extract the sub-matrix but before you do the min(), you could set the diagonal values in the sub-matrix to inf. However you need to pay attention to the boundary conditions as the extracted sub-matrix is not always going to be 3 x 3.

[Ken]

Sorry, also am trying to:

Please implement Breadth First search on the above map from the Robot's position R ("SearchStart") to G ("SearchGoal") and store the distance of each cell to "SearchStart" in the variable "SolutionMap". During expansion, use the 4-neighborhood (i.e., only move vertically and horizontally) and assign each edge a weight (cost of traversal) of "1".

[Walter Roberson]

What you are already doing could be called a Breadth-First Search, under the understanding that you are pruning the search at each step. But it could also be called a Depth-First Search with the same understanding.

More typically, Breadth-First Searches require some form of queuing. At any step, you have a list of nodes to be examined (the first step initialized this list to the starting node), and you run through all of those nodes finding all of the possible next steps, queuing those next steps into a list. So step #1 finds all the nodes 1 step away from the origin, step #2 finds all of the nodes 2 steps away from the origin, and so on. At each step, the number of nodes "deep" in the path is the same for all candidates.

With a Breath-First Search, you would not talk about "back-tracking". However, you do need to consider the termination conditions: you can stop on the first step in which the target is reached (taking the least expensive out of all the routes found with that many steps), but the route that has the least number of steps will not necessarily be the least expensive route. If you are looking for the least expensive route, then the search needs to continue until either you have exhausted all possible routes, or until you can prove that any further route would have to be more expensive than some known route.

A Depth-First search also requires some form of queuing. At any step you pick the least expensive of all of the queued routes that are "in focus", and you find the possible next steps from there and queue those, and shift focus to what you just queued. The queued nodes at any one time will have many different depths. At the point where you know that you cannot go any further (no more candidate nodes that are not already in the tree between the source and the current node) then you pop that focus and move back one step and take the next least expensive and focus on there. That would commonly be referred to as back-tracking.

You have to pay attention to the termination condition for Depth-First Search, too: you might have taken the least expensive route at each point, but that could have led you the long way around. Sometimes the least expensive overall route might involve going up the steepest hill, and that would not be the route that would be found first by Depth-First. For example, traveling between x and y on

   333
   x5y

the least expensive from x (with diagonals not allowed) would be up to the 3, then across to the second 3, then across to the third 3, then down to the y, for a path total of 9; in this case it is cheaper overall to take the more expensive 5 step.

The algorithm you were using originally has no queuing at all, and so can easily get stuck: it had no provision for saying "Oops, dead end! Let's go back a few steps and make a different decision!"

There is no trivial modification of the previous algorithm to make it Depth First or Breadth First: you have to design in the queuing (or "stack").

[Ken]

Thanks Walter. Tried this but get error "Index exceeds matrix dimensions":

% initialize search start and goal locations
SearchStart = [3,7];
SearchGoal  = [9,6];
CurrPos = SearchStart;
% iteratively solve for path from start to goal
      min = abs(-5);

      for x=3:1:size(SMap,1)
       for y=7:1:size(SMap,2)

               if and(SMap(x,y)~=0,SMap(x,y)~=-1)
                       
                  if min > abs(SearchGoal(1) - CurrPos(1)+ 1 + SearchGoal(2) - CurrPos(2));
                      min = abs(SearchGoal(1) - CurrPos(1)+ 1 + SearchGoal(2) - CurrPos(2));
                      min1=min
                      x=min1(1)+1;
                      y=min1(2)+1;
                  end
                      if min > abs(SearchGoal(1) - CurrPos(1) - 1 + SearchGoal(2) - CurrPos(2));
                          min = abs(SearchGoal(1) - CurrPos(1) - 1 + SearchGoal(2) - CurrPos(2));
                          min2=min
                          x=min2(1)+1;
                      y=min2(2)+1;
                      end
                      if min > abs(SearchGoal(1) - CurrPos(1) + SearchGoal(2) - CurrPos(2)+1);
                          min = abs(SearchGoal(1) - CurrPos(1) + SearchGoal(2) - CurrPos(2)+1);
                          min3=min
                          x=min3(1)+1;
                      y=min3(2)+1;
                      end
                       if min > abs(SearchGoal(1) - CurrPos(1) + SearchGoal(2) - CurrPos(2)-1);
                           min = abs(SearchGoal(1) - CurrPos(1) + SearchGoal(2) - CurrPos(2)-1);
                           min4=min
                           x=min4(1)+1;
                      y=min4(2)+1;
                       end                    
                  if SolutionMap(x,y) == 0
                  end
                  end            
               end     
             end

[Ken]

Any hope of a suggestion? Thanks

[Walter Roberson]

What is the point of

        min = abs(-5);

?

And do not use 'min' as a variable name: you are almost certain to slip up and try to also use min as a function. And if you do not, then those of us trying to read your code certainly are confused by it.

[Ken]

That is a mistake, should be least = 5; Still cannot get the solution and keep getting "index exceeds matrix dimensions"

[Walter Roberson]

I do not understand your code. The way you include the position information in the calculation of "min" does not make sense to me.

[Ken]

I start at StartSearch and move towards Searchgoal. I find the distance to the goal by subtracting current position (CurrPos) from SearchGoal. I then move towards SearchGoal by moving 1 cell horiz and one cell vert. I stop when I reach SearchGoal. There are 4 cells neighboring CurrPos. I evaluate each of these for proximity to SearchGoal and choose the cell that has a lowest cost i.e. closest of the 4 cells to SearchGoal.

[Walter Roberson]

Your code uses a key variable SMap which you have not defined in your previous postings, so I am not able to test your code.

[Ken]

These lines of code that I sent on Dec 16 are to be deleted...

% initialize search start and goal locations
SearchStart = [3,7];
SearchGoal  = [9,6];
CurrPos = SearchStart;
% iteratively solve for path from start to goal
      min = abs(-5);

They should be replaced by:

% initialize map, search start and goal as well as the solution map
Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) = -1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;

SearchStart = [3,7];
SearchGoal  = [9,6];

SolutionMap = Inf*ones(size(Map)); %store g-values in here.

So, SMap is a typo and should be SolutionMap

[Walter Roberson]

I have attached the cleaned-up source so that we are working with common code, instead of my having to guess about what code you might have.

I had to restore the initialization of "min".

I renamed "min" to "least".

I removed extra semi-colons and added semi-colons where recommended.

I did not change the algorithm.

Your code has

for x=3:1:size(SolutionMap,1)

and inside of that it has

    x=least1(1)+1;

You should not change a loop control variable inside of the loop, not unless you really know what you are doing. When you change the value of a loop control variable, then as soon as you start the next iteration, the change is undone and the loop control variable is assigned whatever it would have been if you had not changed the variable.

[Ken]

Thanks Walter.

Still get the same error - index exceeds matrix dimensions. I guess the problem is how to find the least cost cell of the 4 that surround the current cell. Tried with no luck

least = min(SolutionMap(x-1,y),SolutionMap(x+1,y));                               SearchSolution(x,y-1), ...
                         SolutionMap(x,y-1),SolutionMap(x,y+1) );

[Walter Roberson]

Including code to protect against going outside of the array:

nrow = size(SolutionMap,1);
ncol = size(SolutionMap,2);

idx = sub2ind([nrow, ncol], x, y);
candidates = [];
if x > 1; candidates = [candidates, idx - 1]; end  %(x-1,y)
if x < nrow; candidates = [candidates, idx + 1]; end  %(x+1, y)
if y > 1; candidates = [candidates, idx - nrow]; end  %(x, y-1)
if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
[least, whichleast] = min( SolutionMap(candidates) );
[newx, newy] = ind2sub([nrow, ncol], candidates(whichleast));

This code makes use of linear indexing to avoid doing a bunch of sub2ind(). It builds up a list of linear indices of locations that are permitted to be examined, finds the minimum of the map at those permitted locations, and turns the found linear index back to row and column index.

[Ken]

Thanks Walter. Your code works fine by itself but when I fit it into the algorithm, get errors:

 Map = zeros(11,9);
 Map(1,:) = -1; Map(11,:) -1; Map(:,1) = -1; Map(:,9)     = -1;
 Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;

 SearchStart = [3,7];
 SearchGoal  = [9,6];
 CurrPos = SearchStart;
 SolutionMap = Inf*ones(size(Map)); %store g-values in here.
 x=3;y=7;

 nrow = size(SolutionMap,1);
ncol = size(SolutionMap,2);
 while ~isequal(SolutionMap(x,y),SearchGoal) 

 if SearchGoal(1) > CurrPos(1);x = x + 1; 
 end

 if SearchGoal(1) < CurrPos(1);x = x - 1;
 end
 if SearchGoal(2) > CurrPos(2);y = y + 1;
 end
 if SearchGoal(2) < CurrPos(2);y = y - 1;
 end
 idx = sub2ind([nrow, ncol], x, y);
 candidates = [];
 if x > 1; candidates = [candidates, idx - 1]; end;%(x-1,y)
[candidates, idx - 1];
 if x < nrow; candidates = [candidates, idx + 1]; end  %(x+1, y)
 [candidates, idx + 1];
 if y > 1; candidates = [candidates, idx - nrow]; end  %(x, y-1)
 [candidates, idx - nrow];
 if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
 [candidates, idx + nrow];
 [least, whichleast] = min( SolutionMap(candidates) );
 [newx, newy] = ind2sub([nrow, ncol], candidates(whichleast));
 x = newx;
 y = newy;
 end

[Walter Roberson]

You did not post a copy of the error.

One thing I notice is that when you read the following statements together:

   if SearchGoal(1) > CurrPos(1);x = x + 1; 
   end

   if SearchGoal(1) < CurrPos(1);x = x - 1;
   end
   if SearchGoal(2) > CurrPos(2);y = y + 1;
   end
   if SearchGoal(2) < CurrPos(2);y = y - 1;
   end

then you can end up moving on the diagonal, which is prohibited in your case.

[Walter Roberson]

Where are you modifying SearchGoal or CurrPos or SolutionMap? You always test fixed locations in SearchGoal and CurrPos but you do not modify either variable. Also you are testing values in SolutionMap but you initialized SolutionMap to all inf

Also notice

   while ~isequal(SolutionMap(x,y),SearchGoal)

SolutionMap(x,y) is going to be one particular entry in SolutionMap, but SearchGoal is a vector.

[Ken]

Thanks Walter. Not sure if the 4 statements I added i.e. "if SearchGoal(1) > CurrPos(1);x = x + 1; end" etc. are of any use since your code finds the minimum of the 4 cells around the current cell. Then the next step should be to move from that minimum cell to SearchGoal. Also, not sure if the min cell has to be calculated for SearchStart as we could move breadthwise until CurrPos(2) = SearchGoal(2), so the y-coordinates are matched. Then move vert. down until the x-coordinates are matched. The error I got was "Out of range subscript"

[Walter Roberson]

I need an exact copy of the error message showing where it occurred.

Remember, I am working on many different questions simultaneously, so if you do not want me to skip yours for being too much trouble, you are advised to post complete error messages, and (when appropriate) complete code. I should not have to reverse-engineer your changes based upon your vague description. I might also be away from my laptop and reading through my phone, in which case I might not even have access to MATLAB at the time -- but a complete copy of the error message might allow me to figure out it mentally instead of having to run the code to see what happens.

You should always be striving to make your questions as easy as practical for the volunteers to understand.

[Ken]

1. I did some mods since I posted my code, so here is the modified code:
2. Map = zeros(11,9);
3. Map(1,:) = -1; Map(11,:) -1; Map(:,1) = -1; Map(:,9) = -1;
4. Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;
6. SearchStart = [3,7];
7. SearchGoal = [9,6];
8. CurrPos = SearchStart;
9. SolutionMap = Inf*ones(size(Map)); %store g-values in here.
10. x=3;y=7;
12. nrow = size(SolutionMap,1);
13. ncol = size(SolutionMap,2);
14. while ~isequal(SolutionMap(x,y),SearchGoal)
16. if SearchGoal(1) > CurrPos(1);x = x + 1;
17. end
19. if SearchGoal(1) < CurrPos(1);x = x - 1;
20. end
21. if SearchGoal(2) > CurrPos(2);y = y + 1;
22. end
23. if SearchGoal(2) < CurrPos(2);y = y - 1;
24. end
25. idx = sub2ind([nrow, ncol], x, y);
26. candidates = [];
27. if x > 1; candidates = [candidates, idx - 1]; end;%(x-1,y)
28. [candidates, idx - 1];
29. if x < nrow; candidates = [candidates, idx + 1]; end %(x+1, y)
30. [candidates, idx + 1];
31. if y > 1; candidates = [candidates, idx - nrow]; end %(x, y-1)
32. [candidates, idx - nrow];
33. if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
34. [candidates, idx + nrow];
35. [least, whichleast] = min( SolutionMap(candidates) );
36. [newx, newy] = ind2sub([nrow, ncol], candidates(whichleast));
37. x = newx;
38. y = newy;
39. end
40. %[newx, newy]
41. % visualize the solution map (g values)
42. imagesc(SolutionMap)
43. set(gca,'dataAspectRatio',[1 1 1])

The error msg is pasted below:

Error using sub2ind (line 55) Out of range subscript.

Error in Untitled2 (line 25) idx = sub2ind([nrow, ncol], x, y);

[Walter Roberson]

Please remove the '#' that you put at the beginning of the lines. I have removed those from your previous postings but it is time consuming to do. Your code cannot be run the way it is.

When you post code, you should highly the code section and click on the '{} Code' button, not the '1 --- 2 --- 3 ---' button.

[Walter Roberson]

Your code

if SearchGoal(1) > CurrPos(1);x = x + 1;

(and related lines) do not check for running off the edge of the array. Those lines are probably not useful, as the code I suggested already handles looking in the appropriate directions.

[Ken]

Thanks Walter. I deleted the 4 SearchGoal lines and now have just the lines for the question and the lines of code from you. What's missing is - once the least value is found (of the 4 cells surrounding the current cell) how to proceed towards SearchGoal from the current cell.

Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) -1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;
SearchStart = [3,7];
SearchGoal  = [9,6];
CurrPos = SearchStart;
SolutionMap = Inf*ones(size(Map)); %store g-values in here.
nrow = size(SolutionMap,1);
ncol = size(SolutionMap,2);
while ~isequal(SolutionMap(x,y),SearchGoal) 
idx = sub2ind([nrow, ncol], x, y);
candidates = [];
if x > 1; candidates = [candidates, idx - 1]; end;%(x-1,y)
[candidates, idx - 1];
if x < nrow; candidates = [candidates, idx + 1]; end  %(x+1, y)
[candidates, idx + 1];
if y > 1; candidates = [candidates, idx - nrow]; end  %(x, y-1)
[candidates, idx - nrow];
if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
[candidates, idx + nrow];
[least, whichleast] = min( SolutionMap(candidates) );
[newx, newy] = ind2sub([nrow, ncol], candidates(whichleast));
x = newx;
y = newy;
end

[Ken]

I added the foll lines at the end but still no luck. The aim was to move from current cell to SearchGoal:

x = newx;
if x > 9
 x=x-1;
 end
 if x < 9
 x=x+1
 end
 if x > 9
 x=x-1;
 end
 y = newy;
  if y < 6
 y =y + 1
 end
 if y > 6
 y =y - 1
 end
 end
(Wonder if this line is the culprit)
while ~isequal(SolutionMap(x,y),SearchGoal)

[Walter Roberson]

newx, newy is the new location.

If following the minima of the 4 surrounding locations does not eventually get you to the goal, then you need a new algorithm. For example you might need an algorithm which backtracks. I described suitable breadth first search and depth first search algorithms above.

[Ken]

Thanks Walter for your patience.

Yes, I read your post on breadth first search but could not relate it to this problem i.e. if my start is 3,7 and goal is 9,6 do I go horizontally left i.e. breadthwise till I get to 6 and then vertically down i.e. depthwise till I get to 9?

[Walter Roberson]

For breadth-first search, at each step you have to make a "hypothesis" that each of the possibilities is the right way to go, so you enter into a list the chain with each of the possible moves relative to where you are focusing on. When one of the chains gets stuck, remove it from the list. Keep going until either you have found one possibility or else until you have accounted for all possibilities. If you find one possibility then it will be a version with the minimum number of steps, but it will not necessarily be the least expensive route. If you cover all of the possibilities then you can pick the least expensive amount them.

For example,

while ~isempty(active_chains)
  new_chains = {};
  for K = 1 : length(active_chains)
    this_chain = active_chains{K};
    if this_chain ends at the goal then
      successful_chains{end+1} = this_chain;
    else
      find all steps that are immediately legal on this_chain,
      discarding any potential step that would go outside the
      matrix and discarding any potential step that would
      re-visit a location that is already present in this_chain.
      Any potential step you find,
         new_chains{end+1} = [this_chain, the next step]
     end
   end
   active_chains = new_chains;
 end

Eventually you will get to the point where there are no further steps possible in any chain and so new_chains will be empty and so active_chains will become empty. At that point, successful_chains will contain all of the possible routes. You can then figure out which of them is least expensive.

[Ken]

Thanks Walter.

I don't think I can do this as it is too complex for me. The course has outlined these steps:

   BF(Graph G, Node Start, Node Goal) 
     Queue.init(FIFO)
     Queue.push(Start) 
     while Queue is not empty:
     Node curr = Queue.pop() 
            if curr is Goal return 
            Closed.push(curr) 
         Nodes next = expand(curr) 
            for all next not in Closed: 
                Queue.push(next)

Some additional info:

Breadth-first search is expanding nodes according to a FIFO queue. At every iteration of your loop you should expand the next node in the queue, and your loop should end when you are about to expand the SearchGoal node, since it would mean you reached the goal position. The distance stored for each cell represents the sum of the weights of the edges from SearchStart to that cell. Please note that in this specific problem each edge has a weight of 1.

[Walter Roberson]

The code I gave is an implementation of that structure, pretty much.

curr = Queue.pop()

would be the same as

curr = active_chains{1};
active_chains(1) = [];

and

Queue.push(value)

would be the same as

active_chains{end+1} = value;

[Ken]

Thanks Walter. Guess my challenge - where I am stuck for the last little while is to adapt the program you sent me earlier (Dec 20) to do this and get the correct solution.

nrow = size(SolutionMap,1);
ncol = size(SolutionMap,2);
idx = sub2ind([nrow, ncol], x, y);
candidates = [];
if x > 1; candidates = [candidates, idx - 1]; end;%(x-1,y)
[candidates, idx - 1];
if x < nrow; candidates = [candidates, idx + 1]; end  %(x+1, y)
[candidates, idx + 1];
if y > 1; candidates = [candidates, idx - nrow]; end  %(x, y-1)
[candidates, idx - nrow];
if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
[candidates, idx + nrow];
[least, whichleast] = min( SolutionMap(candidates) );
[newx, newy] = ind2sub([nrow, ncol], candidates(whichleast));

[Walter Roberson]

The section

candidates = [];
if x > 1; candidates = [candidates, idx - 1]; end;%(x-1,y)
[candidates, idx - 1];
if x < nrow; candidates = [candidates, idx + 1]; end  %(x+1, y)
[candidates, idx + 1];
if y > 1; candidates = [candidates, idx - nrow]; end  %(x, y-1)
[candidates, idx - nrow];
if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
[candidates, idx + nrow];

corresponds to expand(curr), I think.

I am not clear as to what "next not in Closed" is intended to mean. (I have an idea of what it might mean, but if my idea is correct then I have to think more about how the queue is working.)

[Ken]

I am pasting the problem below just in case.

However, the algorithm selects the next cell and if that cell is not to be expanded (because it may be an obstacle or may have been previously expanded), it goes to the closed list not to be expanded.

In the obstacle map shown below black cells represent obstacles, whereas white cells are freely traversable. We define the map's origin at the top left corner, corresponding to cell (1,1). The first dimension faces downward, and the second dimension to the right. The robot and goal locations are marked by "R" and "G", respectively. Note that the map is assumed to have been inflated by the Robot's radius already. It thus represents the configuration space, and planning can proceed for a point-sized robot directly.

Please implement Breadth First search on the above map from the Robot's position R ("SearchStart") to G ("SearchGoal") and store the distance of each cell to "SearchStart" in the variable "SolutionMap". During expansion, use the 4-neighborhood (i.e., only move vertically and horizontally) and assign each edge a weight (cost of traversal) of "1".

[Ken]

Not sure what happened - got msg that you had edited 2 hours back but did not see anything.

[Walter Roberson]

I reformatted what you posted, making it into paragraphs.

[Ken]

Still stuck! Any help appreciated.

[Walter Roberson]

Well, variable.Queue(value) for FIFO is equivalent to

variable(end+1) = value;

and variable.Pop() for a FIFO is equivalent to

result = variable(1);
variable = variable(2:end);

and "value in Closed" is equivalent to

ismember(value, Closed)

[Ken]

Thanks Walter - don't think I know how to fit this into the code you sent on Dec 20.

Also, not sure whether to follow the closed and FIFO strictly. After all, closed means you don't use the cell in closed for future searches. FIFO means you put the next cell in the queue OPTIMALPATH. So, maybe could just try taking each cell from SearchGoal putting it in the queue, taking the 4 neighboring cells to see which has the least cost to SearchStart and putting this least cost cell in OptimalPath?

[Walter Roberson]

The code I gave is not relevant. You need to follow the structure they gave with the Queue stuff.

The description says that Closed should contain all of the blocked points together with all of the places you have already visited.

The costs are not relevant either as the problem statement says to assume that the cost of each link is 1.

The code you are being asked to develop is effectively code to find a path with the smallest number of steps.

[Ken]

Thanks Walter. Tried this but get "Subscript indices must either be real positive integers or logicals."

% initialize map, search start and goal as well as the solution map
Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) = -1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;
SearchStart = [3,7];
SearchGoal  = [9,6];
  SolutionMap = Inf*ones(size(Map)); %store g-values in here.  

OptimalPath = SearchStart;
while ~isequal(OptimalPath(end,:),SearchGoal)
  % extract index corresponding to the minimal
    % value 

      for x=1:1:size(Map,1)
        for y=1:1:size(Map,2)
            if SolutionMap(x,y)~=-1
              NextSolutionMap(x,y) = min(SolutionMap(x-1,y) + ...
                                               SolutionMap(x+1,y) + ...
                                 SolutionMap(x,y-1) + ...
                                             SolutionMap(x,y+1) );
            end
          end
      end

       SolutionMap = NextSolutionMap;
  end

[Ken]

Just to clarify; the lines below (given in program above) are given as part of the problem statement:

% initialize map, search start and goal as well as the solution map
Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) = -1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;

SearchStart = [3,7];
SearchGoal  = [9,6];

SolutionMap = Inf*ones(size(Map)); %store g-values in here.

% BF search
...;

[Walter Roberson]

You should not be using a double for loop. You should be using the Queue structure that they told you to use.

[Ken]

Guess back to square one as this has been my problem since I started attempting this i.e. how to access the cells of the map without the double for loops. My posts of Dec 21 did not have the for loops and they also did not work.

[Walter Roberson]

Look back at the algorithm you gave in http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_416044 . It contains no double-nested for loops. It contains

Nodes next = expand(curr)

which knows which location it is at now and looks only at the locations adjacent to it -- which as I showed before does not require any for loop to produce the candidates.

[Ken]

I tried the

"curr = Queue.pop()
%would be the same as
curr = active_chains{1};
active_chains(1) = [];"

in the foll. lines of the problem:

% initialize map, search start and goal as well as the solution %map
Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) = -1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;
SearchStart = [3,7];
SearchGoal  = [9,6];
 active_chains = [];      
SolutionMap = Inf*ones(size(Map)) %store g-values in here.
curr = active_chains{1};
active_chains(1) = [];

but get error "Cell contents reference from a non-cell array object."

[Walter Roberson]

You need to initialize

   active_chains = {};

an you missed out on doing the initial Push. A push for a FIFO is equivalent to

achive_chains{end+1} = new_value;

[Ken]

Tried this but get the famous 'index exceeds matrix dimensions'

Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) = -1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;

SearchStart = [3,7];
SearchGoal  = [9,6];
 active_chains = {};      
SolutionMap = Inf*ones(size(Map)) %store g-values in here.
curr = active_chains{1};
active_chains(1) = {};

% BF search

while ~isempty(active_chains)
  new_chains = {};
  for K = 1 : length(active_chains)
    this_chain = active_chains{K};
    if this_chain ends at the goal then
      successful_chains{end+1} = this_chain;
    else
      find all steps that are immediately legal on this_chain,
      discarding any potential step that would go outside the
      matrix and discarding any potential step that would
      %re-visit a location that is already present in this_chain.
      %Any potential step you find,
         new_chains{end+1} = [this_chain, the next step]
     end
   end
   active_chains = new_chains;
 end

[Walter Roberson]

Your code does

curr = active_chains{1};

before having pushed on the start information.

[Ken]

Tried this but now get error on line 'if new_chains{end+1} = [this_chain, the next step]'

The expression to the left of the equals sign is not a valid target for an assignment.

active_chains{end+1} = new_value; 
curr = active_chains{1};
active_chains(1) = {};

[Walter Roberson]

A bunch of what is written in that code needs to be in the form of comments, or translated from pseudocode into MATLAB.

[Ken]

In the foll. line, tried to do "the next step" in matlab - could not find any post showing this.

new_chains{end+1} = [this_chain, the next step]

I guess that the min of the 4 surrounding cells would be the next step after checking for no repeats. Your code takes care against it going outside the matrix so don't have to add anything for that.

find all steps that are immediately legal on this_chain,
      discarding any potential step that would go outside the
      matrix and discarding any potential step that would
      re-visit a location that is already present in this_chain.
      Any potential step you find,
         new_chains{end+1} = [this_chain, the next step]

[Ken]

From your earlier post:

"More typically, Breadth-First Searches require some form of queuing. At any step, you have a list of nodes to be examined (the first step initialized this list to the starting node), and you run through all of those nodes finding all of the possible next steps, queuing those next steps into a list. So step #1 finds all the nodes 1 step away from the origin, step #2 finds all of the nodes 2 steps away from the origin, and so on. At each step, the number of nodes "deep" in the path is the same for all candidates."

Not sure I did this i.e. finding nodes 1 step from the origin, 2 steps etc. Also should we start from searchstart and not the origin (1,1)? Also get error (Error in Untitled (line 34) active_chains(1) = {}; active_chains = {}; active_chains{end+1} = SearchStart; curr = active_chains{1}; active_chains(1) = {};

[Walter Roberson]

Translate the imposed algorithm into MATLAB:

function BF(G, Start, Goal)
   Queue = Queue_init_FIFO();
   Closed = Queue_init_FIFO();

     Queue = Queue_push_FIFO(Queue, Start);

     while ~Queue_isempty_FIFO(Queue)
       [curr, Queue] = Queue_pop_FIFO(Queue)
       if isequal(curr, Goal)
         return
       end
       Closed = Queue_push_FIFO(Closed, curr);
       next = Graph_expand(G, curr);
       for K = 1 : size(next, 1)
         this_next = next(K,:);
         if ~Queue_ismember_FIFO(Closed, next)
           Queue = Queue_push_FIFO(Queue, this_next);
         end
       end
     end

Now write the routines Queue_init_FIFO, Queue_push_FIFO, Queue_isempty_FIFO, Queue_pop_FIFO, Queue_ismember_FIFO, and Graph_expand

After that you just have to deal with the problem that your required algorithm doesn't return anything particular, so all you know is that you reached the goal, not how you got there.

[Ken]

Thanks Walter.

Tried:

Queue_init_FIFO:

Queue(1) = []

Queue(2:end) = []

Queue_push_FIFO(value):

Queue[end + 1] = value

Queue_isempty_FIFO:

If Queue(1:end) = []

Queue_isempty_FIFO = 1

Queue_pop_FIFO:

Curr = Queue(1)

Queue(1) = []

Queue_ismember_FIFO(next):

For n = 1:len(Queue)

if Queue(n) ~= next

continue

end

end

Graph_expand:

(Not sure how to do this)

[Walter Roberson]

The lines

Queue[end + 1] = value

If Queue(1:end) = []

are not valid MATLAB code. MATLAB does not use [] for indexing, and MATLAB uses == for testing equality. Also using == [] as a comparison will always give false as the result.

The line

Queue(1) = []

is only valid if Queue exists already, in which case it deletes the first element of Queue.

Queue_ismember_FIFO needs to give back a true or false result, but instead your code just "falls off the end".

You need to write real code with real functions. And you can test them even if you do not know yet how to write the Graph_expand function.

You have a design decision to make: will your Goal and Start and curr and next values be coordinate pairs, or will they be linear indices into the array? Your choice will determine the data structure to use to represent your queue, and will determine how you write your Queue_ismember_FIFO code.

[Ken]

Thanks Walter

Queue[end + 1] = value now changed to Queue (end + 1) = value

If Queue(1:end) = [] now changed to If Queue(1:end) == [] Problem here is that I want to check if it is empty but not sure what to use instead of []

Not clear on: Queue(1) = [] is only valid if Queue exists already, in which case it deletes the first element of Queue

Changed the foll. lines:

For n = 1:length(Queue)

if Queue(n) = next

Queue_ismember_FIFO(next) = true

continue

end

end

Goal and Start and current and next values are coordinate pairs

[Walter Roberson]

Use isempty() to test if something is empty.

Queue (end + 1) = value

that will work if value is a scalar, or if you initialized Queue as a cell array and value is a cell array. However, you have chosen the style of making the value into a coordinate pair. In order to be able to store a vector into a single location like Queue(end+1) then you need to be using cell arrays. You would then use

Queue{end+1} = value;

Your line

if Queue(n) = next

is not syntactically valid in MATLAB. MATLAB uses == for comparisons.

Remember that you have defined next to be a vector, but your Queue(n) is going to be either a scalar (if you define the queue to hold scalars) or a cell array.

And remember to read carefully:

https://www.mathworks.com/help/matlab/ref/if.html

"if expression, statements, end evaluates an expression, and executes a group of statements when the expression is true. An expression is true when its result is nonempty and contains only nonzero elements (logical or real numeric). Otherwise, the expression is false."

You need to be careful using if with vectors: when you are working with vectors you should seriously consider using any() or all(), even if only to re-assure the people reading your code that you knew exactly what you wanted to happen.

[Ken]

Thanks Walter. Tried this but in light of your comments about if, maybe using any() or all() would be better but not sure how to use these.

For n = 1:length(Queue)

if Queue(n) == next

Queue_ismember_FIFO(next) = true

continue

end

end

Also changed:

Queue (end + 1) = value to Queue {end + 1} = value 
Queue(n) to Queue{n}

Queue_isempty_FIFO:

If isempty(Queue(1:end))

Queue_isempty_FIFO = true

[Walter Roberson]

function tf = Queue_ismember_FIFO(Queue, value)
  tf = any( cellfun(@(entry) isequal(entry, value), Queue) );

[Walter Roberson]

function tf = Queue_isempty_FIFO(Queue)
  tf = isempty(Queue);

[Ken]

Thanks Walter.

To recap/summarize:

Queue_init_FIFO: Queue{1} = []

Queue{2:end} = []

Queue_push_FIFO(value):

Queue{end + 1} = value

Queue_isempty_FIFO:

If isempty(Queue{1:end})

Queue_isempty_FIFO = true

Queue_pop_FIFO: Curr = Queue{1}

Queue{1} = []

Queue_ismember_FIFO{next}: function tf = Queue_ismember_FIFO(Queue, value) tf = any( cellfun(@(entry) isequal(entry, value), Queue) );

Graph_expand:

(Not sure how to do this)

[Walter Roberson]

You barely seem to be making any attempt.

function Queue = Queue_init_Fifo
  Queue = {};

function Queue = Queue_push_FIFO(value)
  Queue{end+1} = value;

function tf = Queue_isempty_FIFO(Queue)
  tf = isempty(Queue);

function [value, Queue] = Queue_pop_FIFO(Queue)
  value = Queue{1};
  Queue(1) = [];

function tf = Queue_ismember_FIFO(Queue, value)
  tf = any( cellfun(@(entry) isequal(entry, value), Queue) );

These are simple (except perhaps the ismember, which could have been done with an easy loop.)

I am not going to write the Graph_expand code for you. Or rather, I am not going to re-write the Graph_expand code for you. It is simple code: you know where you are, and you just have to check the four directions to see if they are off the edge of the matrix or if they are marked as blocked, and if they exist and are not blocked you return the index pair for each.

[Ken]

Thanks Walter.

Tried this for Graph_expand:

nrow = size(SolutionMap,1);
    ncol = size(SolutionMap,2);

            if and(Map(x,y)~=-1,Map(x,y)~=SearchGoal)
                         
      idx = sub2ind([nrow, ncol], x, y);
      candidates = [];
      if x > 1; candidates = [candidates, idx - 1]; end  %(x-1,y)
      if x < nrow; candidates = [candidates, idx + 1]; end  %(x+1, y)
      if y > 1; candidates = [candidates, idx - nrow]; end  %(x, y-1)
      if y < ncol; candidates = [candidates, idx + nrow]; end %(x, y+1)
      [least, whichleast] = min( SolutionMap(candidates) );
      [newx, newy] = ind2sub([nrow, ncol], candidates(whichleast));
      end

[Walter Roberson]

No, you should not be taking min() for this situation, you should be returning all of them.

[Ken]

Thanks Walter; so, omitted the last 2 lines and added return.

    if x > 1; candidates = [candidates, idx - 1]; return; end  %(x-1,y)

          if x < nrow; candidates = [candidates, idx + 1]; return; end  %(x+1, y)

          if y > 1; candidates = [candidates, idx - nrow]; return; end  %(x, y-1)

          if y < ncol; candidates = [candidates, idx + nrow]; return; end %(x, y+1)

[Ken]

Hi Walter,

Guess no comment means OK?

[Walter Roberson]

You had not asked a question. I presume you implemented that as a function and tested it and put everything together and tested that and everything worked as well as possible considering that the algorithm you are expected to use has no way to return a solution when it is found.

[Ken]

I tried these function statements in MATLAB but get errors Error: Line: 14 Column: 1 Function definitions in a script must appear at the end of the file. Move all statements after the "Queue_init_Fifo" function definition to before the function definition. :

Queue = {};
function Queue = Queue_init_Fifo

end
Queue{end+1} = value;
function Queue = Queue_push_FIFO(value)

end

[Walter Roberson]

"Function definitions in a script must appear at the end of the file."

That should not be a problem because you should not be programming a script. Everything I showed you was in the form of a function. Put each function into its own .m (so that you can make use of the functions for future projects) and call upon the functions from code that creates your map.

[Ken]

Thanks Walter. I combined the problem statement with the functions as you said and now no error. However (as you pointed out), not sure if I reach the Goal. Any ideas to prove this would be appreciated. When I put this solution in the course problem, get error "•SolutionMap is wrong"

[Walter Roberson]

What is the required form for SolutionMap ?

What is your current code?

[Ken]

SolutionMap:

Code:

% initialize map, search start and goal as well as the solution map
Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) =1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;
SearchStart = [3,7];
SearchGoal  = [9,6];
CurrPos = SearchStart;
SolutionMap = Inf*ones(size(Map)); %store g-values in here.
nrow = size(SolutionMap,1);
ncol = size(SolutionMap,2);
function BF(G, Start, Goal)
Queue = Queue_init_FIFO();
   Closed = Queue_init_FIFO();
     Queue = Queue_push_FIFO(Queue, Start);
     while ~Queue_isempty_FIFO(Queue)
       [currPos, Queue] = Queue_pop_FIFO(Queue)
       if isequal(currPos, Goal)
         return
       end
       Closed = Queue_push_FIFO(Closed, currPos);
       next = Graph_expand(G, currPos);
       for K = 1 : size(next, 1)
         this_next = next(K,:);
         if ~Queue_ismember_FIFO(Closed, next)
           Queue = Queue_push_FIFO(Queue, this_next);
       end
     end
end

imagesc(SolutionMap)
set(gca,'dataAspectRatio',[1 1 1])

           end

[Walter Roberson]

Your code does not call BF, and your code does not change SolutionMap

Is there a reason why you set Map(11,:) =1 when all the other assignments in that section set entries to -1 ?

[Ken]

Thanks Walter. Changed Map(11,:) =-1 I saved the 6 functions in .m and so they are called up in Matlab but not in my problem solution which looks like is in a script and now I get error: "Line: 30 Column: 1 Function definitions in a script must appear at the end of the file. Move all statements after the "BF" function definition to before the function definition."

[Walter Roberson]

"Function definitions in a script must appear at the end of the file."

That should not be a problem because you should not be programming a script. Everything I showed you was in the form of a function. Put each function into its own .m (so that you can make use of the functions for future projects) and call upon the functions from code that creates your map.

[Ken]

Thanks Walter. The error msg I get is from the problem solution (posted yesterday with the robot diagram) which is a script; I do not get error in Matlab as the functions are coded in .m files. Only problem with Matlab is how to check if my path ends at Goal. Also, not sure about: "Your code does not call BF, and your code does not change SolutionMap"

[Walter Roberson]

It is not possible to have an error report about the position of functions in a script file if the script file does not contain any functions.

To be explict: function BF itself should be in its own .m file.

The script code you posted does not have any invocation of BF. Nowhere in the script was there BF(something, somethingelse) as required to call BF with two parameters.

[Ken]

Thanks Walter.

The script file has:

 ncol = size(SolutionMap,2);
  function BF(G, Start, Goal)

BF is in its own .m file, but not sure how it will be accessed from the script in the problem solution as it (.m file) is under the MATLAB dir. I have MATLAB on one computer only.

[Walter Roberson]

The line

    function BF(G, Start, Goal)

defines the function BF, rather than calling it. Remove the word "function" in order to call BF.

I am getting somewhat frustrated with your low efforts. There are lots of examples of how to use functions. https://www.mathworks.com/help/matlab/matlab_prog/create-functions-in-files.html

[Ken]

Thanks Walter; the function part is now OK but I get error. The main program is called Breadth_First_Search and the program that you posted on 31-12 is called BF. Breadth_First_Search calls BF: Undefined function or variable 'x'.

Error in Graph_expand (line 2) if x > 1; candidates = [candidates, idx - 1]; return; end %(x-1,y)

Error in BF (line 13) next = Graph_expand(SolutionMap, CurrPos);

Error in BREADTH_FIRST_SEARCH (line 12) BF(SolutionMap, Start, Goal)

[Ken]

So far silence. Please help!

[Walter Roberson]

The CurrPos that you are passing in to Graph_expand is a pair of values, x and y.

[Ken]

Thanks Walter. I am now testing the functions you gave - pop works fine but not push. Fn is:

function Queue = Queue_push_FIFO(value)
  Queue{end+1} = value;

Tested with: Queue_push_FIFO(5); Queue(end + 1) but get error:

Undefined function or variable 'Queue'.

Error in Queue_push_FIFO (line 3)
  Queue{end+1} = value;

Error in push1 (line 2)
Queue_push_FIFO(5);
I then defined Queue but no luck.

[Walter Roberson]

function Queue = Queue_push_FIFO(Queue, value)
  Queue{end+1} = value;

[Ken]

Thanks Walter. Works fine. Now checking the other functions.

[Ken]

Strangely, when I do this program:

 Queue = {4,5,6}; 
 Q_init_FIFO();

and this function:

function Queue_init_FIFO()

Queue = {};

I get:

Queue =

1×3 cell array

    [4]    [5]    [6]

I then did the same commands one by one using command line and I get the right answer i.e.

Trial>> Queue = {}

 Queue =

0×0 empty cell array

[Walter Roberson]

The line of code I provided was

     Queue = Queue_init_FIFO();

You are failing to assign the output of Queue_init_FIFO to Queue.

[Ken]

Thanks Walter. Works fine.

[Ken]

I tried to test this function:

function tf = Queue_ismember_FIFO(Queue, value)
  tf = any( cellfun(@(entry) isequal(entry, value), Queue) );

with the program:

Queue = {2,1,3};
value = 3;
tf

Should get a true since 3 is a member of Queue but get:

Trial>> tf1

ans =

    Empty transfer function.

[Ken]

When I changed the program to below, it works fine: (However, in this case what is the point of having a function, because the last line is already part of the function)?

value = 3;
Queue = {3,1,5};
any( cellfun(@(entry) isequal(entry, value), Queue) )

[Walter Roberson]

You did not run the function. To test it you need something like

Queue = {2,1,3};
value = 3;
result = Queue_ismember_FIFO(Queue, value)

Remember, when you define a function, the variable on the left of the "=" of the "function" statement is not assigned to in the calling routine unless you deliberately assign it there.

[Ken]

Thanks Water. Works fine. Now to the last fn i.e. Graph_Expand

[Ken]

Tried this (Breadth_First_Search) as the main program which calls FUNCTION BF which calls FUNCTION G_EXP. Get errors: Trial>> BF Not enough input arguments.

Error in BF (line 5) Queue = Queue_push_FIFO(Queue, Start);

Map = zeros(11,9);
Map(1,:) = -1; Map(11,:) =1; Map(:,1) = -1; Map(:,9)     = -1;
Map(9,2) = -1; Map(10,2) = -1; Map(10,3)= -1; Map(5:6,5:8) = -1;
Start = [3,7];
Goal  = [9,6];
CurrPos = Start;
%x = curr(1);
%y = curr(2);
SolutionMap = Inf*ones(size(Map)); %store g-values in here.
nrow = size(SolutionMap,1);
ncol = size(SolutionMap,2);
BF(SolutionMap, Start, Goal)

BF:

function BF(SolutionMap, Start, Goal)
   Queue = Queue_init_FIFO();
   Closed = Queue_init_FIFO();
     Queue = Queue_push_FIFO(Queue, Start);
     while ~Queue_isempty_FIFO(Queue)
       [curr, Queue] = Queue_pop_FIFO(Queue)
       if isequal(curr, Goal)
         return
       end
       Closed = Queue_push_FIFO(Closed, curr);
       next = G_EXP(SolutionMap, curr);
       for K = 1 : size(next, 1)
         this_next = next(K,:);
         if ~Queue_ismember_FIFO(Closed, next)
           Queue = Queue_push_FIFO(Queue, this_next);
         end
       end

G_EXP:

function next = G_EXP(SolutionMap, curr)
x = curr(1);
y = curr(2);
candidates = [];
if x > 1; candidates = [candidates, idx - 1]; return;end;%(x-1,y)
[candidates, idx - 1];
if x < nrow; candidates = [candidates, idx + 1]; return;end  %(x+1, y)
[candidates, idx + 1];
if y > 1; candidates = [candidates, idx - nrow]; return;end  %(x, y-1)
[candidates, idx - nrow];
if y < ncol; candidates = [candidates, idx + nrow]; return;end %(x, y+1)

[Ken]

Also tried using Breadth_First search and got: Trial>> BREADTH_FIRST

curr =

       3     7

Queue =

    1×0 empty cell array

Undefined function or variable 'candidates'.

Error in G_EXP (line 4)
candidates

Error in BF (line 12)
       next = G_EXP(SolutionMap, curr);

Error in BREADTH_FIRST (line 8)
BF(SolutionMap, Start, Goal)

[Ken]

Walter, Walter, where are you? Please help! The posting of 31-12 just does not work.

[Walter Roberson]

You lost an "end" statement in BF.

I had given code for function Queue_init_Fifo instead of the required Queue_init_FIFO, a typo on my part; it appears you noticed and fixed that yourself.

When you created G_EXP you lost the sub2ind() that creates idx. But you should probably just rewrite your G_EXP in terms of building rows of x y pairs instead of indices. Remove the "return" statements while you are there, since you need to construct the complete list. And remember you need to create the output variable, which you have named "next".

You should be passing your Map into BF, not the SolutionMap. Your SolutionMap does not have any information about which places are blocked. The SolutionMap should be an output from the algorithm, but the BF algorithm you were told to use does not have any way to set the SolutionMap entries.

Ah, a bug in my code: in BF, the line

           if ~Queue_ismember_FIFO(Closed, next)

should be

           if ~Queue_ismember_FIFO(Closed, this_next)

[Walter Roberson]

The algorithm you gave in http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_416044 has some flaws:

• It does not check that the current node is eligible for expansion (the expand routine is not intended to do that in the algorithm). This would result in incorrect paths
• the algorithm does not give any clue as to how to create the solution map
• Potential nodes are checked for being closed at the time of expand(), but are not checked when they become current. Nodes can end up being closed after queuing if they become current and are found to be blocked; and whether or not they are found to be blocked, the algorithm defines that they get queued as closed when they become current. Because of this algorithm set-up, a node can end up queued twice. It is inefficient to expand the same node twice. The algorithm should either re-check for closed when a node becomes current or else it should avoid entering anything into the queue when it is already in the queue.

It could make sense to have a node in the queue multiple times if the step costs are variable, or if you were not using a breadth-first search, as the alternate entries could potentially represent a less-expensive route, but in the case of a breadth-first search with constant step cost, it is not possible that the other route might be less expensive.

To deal with the creation of the solution map, I found it effective to add a new queue for the costs. Initialize that queue with a 0. Each time you pop off something from Queue, also pop the current cost from the cost queue. Each time you add something onto the Queue, also queue (1 more than the current cost). When a location becomes current and is not a location that is blocked, then set the solution map at that location to be the current cost (that was popped off the cost queue.) At each point, the N'th element of the Queue and the N'th element of the cost queue are paired, representing the cost to reach that location.

[Ken]

Thanks Walter. Some clarification on the problem: The Breadth-first search method expands nodes according to a First In First Out (FIFO) queue. Once the goal state is expanded, it backtracks the solution from the goal state backwards in a greedy way, based on each node's information about its distance from the start. This specific problem only asks you to implement the first part of the algorithm, thus you only need to build the SolutionMap, containing each cell's distance from the start. To answer your question specifically, the FIFOQueue is not going to contain the optimal path to take from the start to the goal state. It's just a helper variable to keep track of the nodes to expand next. In general, the algorithm consists of the main loop, at every iteration of which you should expand the next node in the queue, and enqueue its neighbouring cells. The main loop should end when you are about to expand the SearchGoal node since it would mean you reached the goal position.

[Ken]

Looks like the BF Algorithm has to be modified.

[Ken]

Modified G_Exp to G_Ex1:

function next = G_Ex1(G, curr)
x=curr(1);y=curr(2);
nrow = size(G,1);
ncol = size(G,2);
%idx = sub2ind([nrow, ncol], x, y);
candidates = [];

if x > 1; candidates = [candidates;[x-1,y]];
end;%(x-1,y)
%[candidates, idx - 1]
if x < nrow; candidates = [candidates;[x+1, y]];
end  %(x+1, y)
%[candidates, idx + 1]

            if y > 1; candidates = [candidates;[x, y-1]];
                 end  %(x, y-1)
                %[candidates, idx - nrow]

              if y < ncol; candidates = [candidates;[x, y+1]];
                 end %(x, y+1)
  %[candidates, idx + nrow]
  next = candidates
  end

[Ken]

Hi Walter,

Any comments on the above? The post of 31-12 did not have any criteria for putting cells in Closed - it just put them there

[Walter Roberson]

Your G_Exp1 looks plausible.

In http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_416142 you wrote,

"However, the algorithm selects the next cell and if that cell is not to be expanded (because it may be an obstacle or may have been previously expanded), it goes to the closed list not to be expanded."

but the algorithm you gave in http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_416044 has just

              Closed.push(curr)

That unconditional push onto the closed list pushes the current item onto the closed list so that later the closed list can be tested to see if the item has already been expanded. Unfortunately that algorithm you were given does not handle the "because it may have been an obstacle" part (there is no test anywhere in the algorithm for the current location being an obstacle), and the algorithm does not properly handle "or may have been previously expanded" because the algorithm given does not test whether the current item is on the closed list, only whether a prospective location is on the closed list. As I described earlier, that can result in a location being put onto the Queue multiple times.

The algorithm you were given that you reproduced in the #comment_416044 is at fault.

[Ken]

Thanks Walter. I was not given that algorithm but did it myself - wrongly as you pointed out. Any suggestions to correct it would be appreciated.

[Walter Roberson]

You specifically said, "The course has outlined these steps", which I interpreted as indicating that you were given that pseudocode.

[Ken]

Thanks Walter. Any suggestions to correct it would be deeply appreciated.

[Walter Roberson]

Please re-read http://www.mathworks.com/matlabcentral/answers/315127-robot-localization-and-motion-planning#comment_419758 as I was strongly hinting at the solutions there.

[Ken]

Thanks Walter. Looked at it - too complex for me!

[Walter Roberson]

Break it into pieces. When I write

"It does not check that the current node is eligible for expansion (the expand routine is not intended to do that in the algorithm). This would result in incorrect paths"

then the obvious solution is that the code should check that the current node is eligible for expansion -- that it is not a blocked location (check within the graph!) and that it has not already been added to the Closed list.