You are now following this question
- You will see updates in your followed content feed.
- You may receive emails, depending on your communication preferences.
ROBOT LOCALIZATION AND MOTION PLANNING
4 views (last 30 days)
Show older comments
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) ????;
3 Comments
Walter Roberson
on 2 Dec 2016
What is the MATLAB question here? This seems to be a doit4me
Ken
on 2 Dec 2016
Edited: Ken
on 2 Dec 2016
- 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
on 2 Dec 2016
Edited: Walter Roberson
on 2 Dec 2016
Some added info:
x = [1.; 2. ; pi/4]
u = [.1; pi/8]
Accepted Answer
Walter Roberson
on 16 Jan 2017
7 Comments
Walter Roberson
on 17 Jan 2017
Sorry this was a test post that I forgot to delete.
Walter Roberson
on 17 Jan 2017
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
on 17 Jan 2017
Edited: Walter Roberson
on 18 Jan 2017
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
on 18 Jan 2017
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
on 18 Jan 2017
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
More Answers (1)
Walter Roberson
on 2 Dec 2016
Assuming your formulae are correct,
Fx = @(x,u) [1;1;1];
Fu = @(x,u) [cos(x(2)+1);sin(x(2)+1);1];
115 Comments
Walter Roberson
on 2 Dec 2016
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
on 3 Dec 2016
Edited: Walter Roberson
on 3 Dec 2016
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
on 3 Dec 2016
Edited: Walter Roberson
on 3 Dec 2016
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
on 3 Dec 2016
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
on 5 Dec 2016
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
on 5 Dec 2016
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
on 8 Dec 2016
Edited: Walter Roberson
on 8 Dec 2016
- 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];
Walter Roberson
on 14 Dec 2016
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
on 14 Dec 2016
Edited: Walter Roberson
on 15 Dec 2016
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
on 15 Dec 2016
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
on 16 Dec 2016
Edited: Walter Roberson
on 18 Dec 2016
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
Walter Roberson
on 18 Dec 2016
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.
Walter Roberson
on 19 Dec 2016
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
on 19 Dec 2016
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
on 19 Dec 2016
Edited: Walter Roberson
on 19 Dec 2016
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
on 19 Dec 2016
Edited: Walter Roberson
on 20 Dec 2016
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
on 20 Dec 2016
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
on 20 Dec 2016
Edited: Walter Roberson
on 20 Dec 2016
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
on 20 Dec 2016
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
on 21 Dec 2016
Edited: Walter Roberson
on 21 Dec 2016
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
on 21 Dec 2016
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
on 21 Dec 2016
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
on 21 Dec 2016
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
on 21 Dec 2016
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
on 21 Dec 2016
Edited: Ken
on 21 Dec 2016
- I did some mods since I posted my code, so here is the modified code:
- 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
- %[newx, newy]
- % visualize the solution map (g values)
- imagesc(SolutionMap)
- 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
on 21 Dec 2016
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
on 21 Dec 2016
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
on 21 Dec 2016
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
on 22 Dec 2016
Edited: Ken
on 22 Dec 2016
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
on 23 Dec 2016
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
on 23 Dec 2016
Edited: Walter Roberson
on 23 Dec 2016
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
on 23 Dec 2016
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
on 23 Dec 2016
Edited: Walter Roberson
on 24 Dec 2016
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
on 24 Dec 2016
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
on 24 Dec 2016
Edited: Ken
on 24 Dec 2016
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
on 24 Dec 2016
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
on 24 Dec 2016
Edited: Walter Roberson
on 24 Dec 2016
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".
Walter Roberson
on 25 Dec 2016
I reformatted what you posted, making it into paragraphs.
Walter Roberson
on 26 Dec 2016
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
on 26 Dec 2016
Edited: Walter Roberson
on 26 Dec 2016
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
on 26 Dec 2016
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
on 26 Dec 2016
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
on 26 Dec 2016
Edited: Walter Roberson
on 26 Dec 2016
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
on 26 Dec 2016
You should not be using a double for loop. You should be using the Queue structure that they told you to use.
Ken
on 27 Dec 2016
Edited: Ken
on 27 Dec 2016
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
on 27 Dec 2016
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
on 28 Dec 2016
Edited: Walter Roberson
on 28 Dec 2016
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
on 28 Dec 2016
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
on 29 Dec 2016
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
on 29 Dec 2016
Your code does
curr = active_chains{1};
before having pushed on the start information.
Ken
on 30 Dec 2016
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
on 30 Dec 2016
Edited: Walter Roberson
on 30 Dec 2016
A bunch of what is written in that code needs to be in the form of comments, or translated from pseudocode into MATLAB.
Ken
on 31 Dec 2016
Edited: Ken
on 31 Dec 2016
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
on 31 Dec 2016
Edited: Ken
on 31 Dec 2016
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
on 31 Dec 2016
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
on 2 Jan 2017
Edited: Walter Roberson
on 2 Jan 2017
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
on 2 Jan 2017
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
on 3 Jan 2017
Edited: Ken
on 3 Jan 2017
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
on 3 Jan 2017
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:
"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
on 3 Jan 2017
Edited: Ken
on 3 Jan 2017
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
on 3 Jan 2017
function tf = Queue_ismember_FIFO(Queue, value)
tf = any( cellfun(@(entry) isequal(entry, value), Queue) );
Walter Roberson
on 3 Jan 2017
function tf = Queue_isempty_FIFO(Queue)
tf = isempty(Queue);
Ken
on 3 Jan 2017
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
on 4 Jan 2017
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
on 4 Jan 2017
Edited: Walter Roberson
on 4 Jan 2017
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
on 4 Jan 2017
No, you should not be taking min() for this situation, you should be returning all of them.
Ken
on 4 Jan 2017
Edited: Ken
on 4 Jan 2017
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)
Walter Roberson
on 5 Jan 2017
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
on 5 Jan 2017
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
on 5 Jan 2017
"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
on 6 Jan 2017
Edited: Ken
on 6 Jan 2017
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
on 6 Jan 2017
What is the required form for SolutionMap ?
What is your current code?
Ken
on 6 Jan 2017
Edited: Walter Roberson
on 6 Jan 2017
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
on 6 Jan 2017
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
on 6 Jan 2017
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
on 6 Jan 2017
"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
on 6 Jan 2017
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
on 6 Jan 2017
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
on 6 Jan 2017
Edited: Walter Roberson
on 6 Jan 2017
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
on 6 Jan 2017
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
on 7 Jan 2017
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)
Walter Roberson
on 9 Jan 2017
The CurrPos that you are passing in to Graph_expand is a pair of values, x and y.
Ken
on 9 Jan 2017
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
on 9 Jan 2017
function Queue = Queue_push_FIFO(Queue, value)
Queue{end+1} = value;
Ken
on 9 Jan 2017
Edited: Walter Roberson
on 9 Jan 2017
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
on 9 Jan 2017
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
on 9 Jan 2017
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
on 10 Jan 2017
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
on 10 Jan 2017
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
on 10 Jan 2017
Edited: Ken
on 11 Jan 2017
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
on 10 Jan 2017
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)
Walter Roberson
on 11 Jan 2017
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
on 11 Jan 2017
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
on 12 Jan 2017
Edited: Ken
on 12 Jan 2017
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
on 12 Jan 2017
Edited: Walter Roberson
on 13 Jan 2017
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
Walter Roberson
on 13 Jan 2017
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.
Walter Roberson
on 14 Jan 2017
You specifically said, "The course has outlined these steps", which I interpreted as indicating that you were given that pseudocode.
Walter Roberson
on 16 Jan 2017
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.
Walter Roberson
on 16 Jan 2017
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.
See Also
Categories
Find more on Marine and Underwater Vehicles in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!An Error Occurred
Unable to complete the action because of changes made to the page. Reload the page to see its updated state.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)
Asia Pacific
- Australia (English)
- India (English)
- New Zealand (English)
- 中国
- 日本Japanese (日本語)
- 한국Korean (한국어)