-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathobstacle.m
307 lines (270 loc) · 12.2 KB
/
obstacle.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
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
classdef obstacle
% This is the obstacle class. It has the following member functions:
%
% constructor
% ===========
% input:
% center0 -- initial starting center of obstacle
% vertices -- the object vertices at the initial state
% v0 -- the initial velocity of the object
% yaw0 -- the initial heading angle of the object
% num_ctrl -- number of control in the planning horizon
% dt -- discretization time step
% w_c -- the cost function weight for the closest distance term
% ds -- safety distance for the object
% t -- log barrier function weight
%
% In this function, an empty trajectory, an empty vertices matrix and zero
% cost function derivatives are initialized along with physical parameters.
% Most importantly, the set of vertices that is centered at the origin with
% zero heading angle is recovered
%
% update_ctrl
% ===========
% input:
% ctrl -- control sequence of the obstacle in the next horizon
%
% In this function, only control sequence is updated
%
% update_traj
% ===========
% In this function, all states and vertices are updated based on the
% control sequence set previously
%
% next_window
% ===========
% input:
% ctrl -- control sequence of the obstacle in the next horizon
%
% In this function, the initial state and vertice set are first initialized
% as the second element in the previous iteration. Then, update_traj is
% called.
%
% load_traj
% =========
% In this function, a trajectory is loaded and corresponding vertices are
% calculated and updated
%
% calc_cost (WIP)
% =========
% In this function, all cost function derivatives are calculated and
% updated.
% I might use finite difference b/c the analytical form of might be hard to
% solve
%% class member variables
properties
vertices0 % polygon vertices when centering on the origin
% (repeating the first vertex)
vertices % veritces at each step
center0 % initial center position, two element array (2x1)
a0 % initial velocity
v0 % initial velocity
yaw0 % initial yaw rate
traj_pred % predicted trajectory
traj % obstacle trajectory (x,y,vel,yaw)
ctrl % obstacle control sequence (U_DIM * NUM_CTRL)
num_ctrl % number of sliding window length
dt % discretization time step
t_switch % switch between short term and long term prediction
L % obstacle wheel base
dist_safe % obstacle avoidance safe distance
ellipse_axis% obstacle ellipse axis
end
%% class member constants
properties (Constant)
X_DIM = 5; % state vector dimension
U_DIM = 2; % input vector dimension
len = 5.0; % vehicle length
wid = 2.0; % vehicle width
lf = 1.5; % Vehicle front wheel distance to CG
lr = 1.5; % Vehicle rear wheel distance to CG
rad = 1.7493; % Vehicle two-circle representation circle radius
t_safe = 3.0; % safe time, second
end
%% member functions
methods
%% constructor
function obj = obstacle(center0, vertices,a0, v0, yaw0, ...
num_ctrl, dt, t_switch)
rotmat = @(theta) [cos(theta), -sin(theta); sin(theta), cos(theta)];
% geometry
obj.center0 = center0;
if (~isempty(vertices))
obj.vertices0 = rotmat(-yaw0) * ...
(vertices - center0 * ones(1, size(vertices, 2)));... recover vertices0
obj.vertices = zeros([size(vertices), num_ctrl+1]);
obj.vertices(:,:, 1) = vertices;... initialize the first set of vertices
end
% initialize the semi major and minor axis for the ellipse
obj.ellipse_axis = diag([obj.len/2*sqrt(2) + obj.len/2,
obj.wid/2*sqrt(2)+ obj.wid/2]) * ones(2, num_ctrl+1);
% states initialization
obj.a0 = a0;
obj.v0 = v0;
obj.yaw0 = yaw0;
obj.num_ctrl = num_ctrl;
obj.dt = dt;
obj.t_switch = t_switch;
% obj.dist_safe = ds;
% trajectory initialization
obj.traj = zeros(obj.X_DIM, num_ctrl + 1);
obj.traj(:,1) = [center0(1); center0(2); a0; v0; yaw0];
obj.ctrl = zeros(obj.U_DIM, num_ctrl);
% update predicted trajectory
obj.traj_pred = zeros(obj.X_DIM, num_ctrl + 1);
obj.L = obj.lf + obj.lr;
end
%% update control sequence
function obj = update_ctrl(obj, ctrl)
obj.ctrl = ctrl;
end
%% update trajectory based on self control sequence and state
function obj = update_traj(obj, flag)
% flag: whether to update vertices
rotmat = @(theta) [cos(theta), -sin(theta); sin(theta), cos(theta)];
for i = 2:obj.num_ctrl + 1
X = obj.traj(:, i-1);
U = obj.ctrl(:, i-1);
% extract information for convenience
x = X(1);
y = X(2);
acc = X(3);
v = X(4);
theta = X(5);
jerk = U(1);
delta = U(2);
curvature = tan(delta) / obj.L;
l = v * obj.dt + 0.5 * acc * (obj.dt)^2 +(1/6.)*jerk*(obj.dt)^3;
% update vehicle state via kinematics
obj.traj(4, i) = v + acc * obj.dt+0.5 * jerk * (obj.dt)^2;... velocity update
obj.traj(3, i) = acc +jerk* obj.dt;... acc update
obj.traj(5, i) = theta + curvature * l;%edited b Omid
if (curvature == 0)
obj.traj(1, i) = x + l * cos(theta);
obj.traj(2, i) = y + l * sin(theta);
else
obj.traj(1, i) = x + (sin(theta + curvature * l) - ...
sin(theta))/curvature;
obj.traj(2, i) = y + (cos(theta) - cos(theta + ...
curvature * l))/curvature;
end
% update vertices
if (flag)
obj.vertices(:, :, i) = rotmat(theta) * obj.vertices(:, :, 1) + ...
([x;y] - obj.center0)*ones(1,size(obj.vertices0, 2));
end
end
end
%% slide the obstacle to the next window
function obj = next_window(obj, ctrl, flag, bounding_box)
% flag: whether to update the obstacle vertices
% bounding_box: 2x4xk vertices of the reachability set
if (~isempty(ctrl))
obj = obj.update_ctrl(ctrl);
end
% update the first state on the trajectory
obj.traj(:,1) = obj.traj(:,2);
if (flag)
% update the first set of vertices
obj.vertices(:,:,1) = obj.vertices(:,:,2);
end
obj = obj.update_traj(flag);
% update the elliptical axis length if bounding box is given
if (~isempty(bounding_box))
for i = 1:(obj.t_switch/obj.dt)
point1 = bounding_box(:,1,i);
point2 = bounding_box(:,2,i);
point3 = bounding_box(:,3,i);
if (point1(1) == point2(1))
dx = abs(point2(1) - point3(1));
dy = abs(point2(2) - point1(2));
else
dx = abs(point2(1) - point1(1));
dy = abs(point2(2) - point3(2));
end
obj.ellipse_axis(:,i) = [dx*sqrt(2)/2 + obj.len/2;
dy*sqrt(2)/2 + obj.wid/2];
end
end
end
%% load a trajectory as the self trajectory and update bounding box
function obj = load_traj(obj, traj, bounding_box, flag)
rotmat = @(theta) [cos(theta), -sin(theta); sin(theta), cos(theta)];
% trajectory is defined by 4xn matrix
obj.traj = traj;
for i = 1:obj.num_ctrl+1
x = obj.traj(1, i);
y = obj.traj(2, i);
theta = obj.traj(3, i);
if (flag == true)
% upate vertices
obj.vertices(:, :, i) = rotmat(theta) * obj.vertices0 + ...
([x;y] - obj.center0)*ones(1,size(obj.vertices0, 2));
end
end
% update the elliptical axis length if bounding box is given
if (~isempty(bounding_box))
for i = 1:(obj.t_switch/obj.dt)
point1 = bounding_box(:,1,i);
point2 = bounding_box(:,2,i);
point3 = bounding_box(:,3,i);
if (point1(1) == point2(1))
dx = abs(point2(1) - point3(1));
dy = abs(point2(2) - point1(2));
% also update the trajectory
obj.traj(1,i) = (point2(1) + point3(1))/2;
obj.traj(2,i) = (point2(2) - point1(2))/2;
else
dx = abs(point2(1) - point1(1));
dy = abs(point2(2) - point3(2));
% also update the trajectory
obj.traj(1,i) = (point2(1) - point1(1))/2;
obj.traj(2,i) = (point2(2) - point3(2))/2;
end
obj.ellipse_axis(:,i) = [dx*sqrt(2)/2 + obj.len/2;
dy*sqrt(2)/2 + obj.wid/2];
end
end
end
%% load a trajectory as the self trajectory and update bounding box
function obj = load_traj_predicted(obj, traj_pred, bounding_box, flag)
rotmat = @(theta) [cos(theta), -sin(theta); sin(theta), cos(theta)];
% trajectory is defined by 4xn matrix
obj.traj_pred = traj_pred;
for i = 1:obj.num_ctrl+1
x = obj.traj_pred(1, i);
y = obj.traj_pred(2, i);
theta = obj.traj_pred(3, i);
if (flag == true)
% upate vertices
obj.vertices(:, :, i) = rotmat(theta) * obj.vertices0 + ...
([x;y] - obj.center0)*ones(1,size(obj.vertices0, 2));
end
end
% update the elliptical axis length if bounding box is given
if (~isempty(bounding_box))
for i = 1:(obj.t_switch/obj.dt)
point1 = bounding_box(:,1,i);
point2 = bounding_box(:,2,i);
point3 = bounding_box(:,3,i);
if (point1(1) == point2(1))
dx = abs(point2(1) - point3(1));
dy = abs(point2(2) - point1(2));
% also update the trajectory
obj.traj_pred(1,i) = (point2(1) + point3(1))/2;
obj.traj_pred(2,i) = (point2(2) + point1(2))/2;
else
dx = abs(point2(1) - point1(1));
dy = abs(point2(2) - point3(2));
% also update the trajectory
obj.traj_pred(1,i) = (point2(1) + point1(1))/2;
obj.traj_pred(2,i) = (point2(2) + point3(2))/2;
end
obj.traj_pred(4,i) = 0;
obj.ellipse_axis(:,i) = [dx*sqrt(2)/2 + obj.len/2;
dy*sqrt(2)/2 + obj.wid/2];
end
end
end
end
end