forked from petercorke/robotics-toolbox-matlab
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathBug2.m
183 lines (160 loc) · 6.56 KB
/
Bug2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
%BUG2 Bug navigation class
%
% A concrete subclass of the abstract Navigation class that implements the bug2
% navigation algorithm. This is a simple automaton that performs local
% planning, that is, it can only sense the immediate presence of an obstacle.
%
% Methods::
% path Compute a path from start to goal
% visualize Display the obstacle map (deprecated)
% plot Display the obstacle map
% display Display state/parameters in human readable form
% char Convert to string
%
% Example::
% load map1 % load the map
% bug = Bug2(map); % create navigation object
% bug.goal = [50, 35]; % set the goal
% bug.path([20, 10]); % animate path to (20,10)
%
% Reference::
% - Dynamic path planning for a mobile automaton with limited information on the environment,,
% V. Lumelsky and A. Stepanov,
% IEEE Transactions on Automatic Control, vol. 31, pp. 1058-1063, Nov. 1986.
% - Robotics, Vision & Control, Sec 5.1.2,
% Peter Corke, Springer, 2011.
%
% See also Navigation, DXform, Dstar, PRM.
% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
classdef Bug2 < Navigation
properties(Access=protected)
H % hit points
j % number of hit points
mline % line from starting position to goal
step % state, in step 1 or step 2 of algorithm
edge % edge list
k % edge index
end
methods
function bug = Bug2(world, goal)
%Bug2.Bug2 bug2 navigation object constructor
%
% B = Bug2(MAP) is a bug2 navigation
% object, and MAP is an occupancy grid, a representation of a
% planar world as a matrix whose elements are 0 (free space) or 1
% (occupied).
%
% Options::
% 'goal',G Specify the goal point (1x2)
% 'inflate',K Inflate all obstacles by K cells.
%
% See also Navigation.Navigation.
% invoke the superclass constructor
bug = bug@Navigation(world);
bug.H = [];
bug.j = 1;
bug.step = 1;
end
% null planning for the bug!
function plan(bug, goal)
bug.goal = goal;
end
function navigate_init(bug, robot)
if isempty(bug.goal)
error('RTB:bug2:nogoal', 'no goal set, cant compute path');
end
% parameters of the M-line, direct from initial position to goal
% as a vector mline, such that [robot 1]*mline = 0
dims = axis;
xmin = dims(1); xmax = dims(2);
ymin = dims(3); ymax = dims(4);
% create homogeneous representation of the line
% line*[x y 1]' = 0
bug.mline = homline(robot(1), robot(2), ...
bug.goal(1), bug.goal(2));
bug.mline = bug.mline / norm(bug.mline(1:2));
if bug.mline(2) == 0
% handle the case that the line is vertical
plot([robot(1) robot(1)], [ymin ymax], 'k--');
else
x = [xmin xmax]';
y = -[x [1;1]] * [bug.mline(1); bug.mline(3)] / bug.mline(2);
plot(x, y, 'k--');
end
end
% this should be a protected function, but can't make this callable
% from superclass when it's instantiation of an abstract method
% (R2010a).
function n = next(bug, robot)
n = [];
% these are coordinates (x,y)
if bug.step == 1
% Step 1. Move along the M-line toward the goal
if colnorm(bug.goal - robot) == 0 % are we there yet?
return
end
% motion on line toward goal
d = bug.goal-robot;
dx = sign(d(1));
dy = sign(d(2));
% detect if next step is an obstacle
if bug.occgrid(robot(2)+dy, robot(1)+dx)
bug.message('(%d,%d) obstacle!', n);
bug.H(bug.j,:) = robot; % define hit point
bug.step = 2;
% get a list of all the points around the obstacle
bug.edge = edgelist(bug.occgrid==0, robot);
bug.k = 2; % skip the first edge point, we are already there
else
n = robot + [dx; dy];
end
end % step 1
if bug.step == 2
% Step 2. Move around the obstacle until we reach a point
% on the M-line closer than when we started.
if colnorm(bug.goal-robot) == 0 % are we there yet?
return
end
if bug.k <= numrows(bug.edge)
n = bug.edge(bug.k,:)'; % next edge point
else
% we are at the end of the list of edge points, we
% are back where we started. Step 2.c test.
error('robot is trapped')
return;
end
% are we on the M-line now ?
if abs( [robot' 1]*bug.mline') <= 0.5
bug.message('(%d,%d) moving along the M-line', n);
% are closer than when we encountered the obstacle?
if colnorm(robot-bug.goal) < colnorm(bug.H(bug.j,:)'-bug.goal)
% back to moving along the M-line
bug.j = bug.j + 1;
bug.step = 1;
return;
end
end
% no, keep going around
bug.message('(%d,%d) keep moving around obstacle', n)
bug.k = bug.k+1;
end % step 2
end % next
end % methods
end % classdef