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
|
function state=car_solve(initial,final)
//
// CAR PACKING VIA FLATNESS AND FRENET FORMULAS
//
// explicit computation and visualisation of the motions.
//
// February 1993
//
// ............................................................
// : pierre ROUCHON <rouchon@cas.ensmp.fr> :
// : Centre Automatique et Systemes, Ecole des Mines de Paris :
// : 60, bd Saint Michel -- 75272 PARIS CEDEX 06, France :
// : Telephone: (1) 40 51 91 15 --- Fax: (1) 43 54 18 93 :
// :..........................................................:
//
// initial: initial position [x,y,theta,phi]
// final : final position [x,y,theta,phi]
// theta : the car angle
// phi : the front wheel angle
bigT = 1 ;//basic time interval for one smooth motion (s)
bigL = 1 ;// car length (m)
// computation of intermediate configuration
x0 = max(initial(1),final(2)) ....
+ bigL*abs(tan(initial(3))) ...
+ bigL*abs(tan(final(3))) ...
+ bigL*(abs(initial(2)-final(2))/bigL)^(1/2) ;
y0 = (initial(2)+final(2))/2 ;
intermediate=[x0,y0,0,0]'
// first polynomial curve
state=[matrix(initial,1,-1);
car_polynomial_curve(initial,intermediate,"direct")]
//
// second polynomial curve
state = [ state;
matrix(intermediate,1,-1)
car_polynomial_curve(final,intermediate,"reverse")
matrix(final,1,-1)]
endfunction
function state=car_polynomial_curve(initial,final,orient)
nbpt = 40 ; // sampling of motion
theta1 = initial(3) ; phi1 = initial(4) ;
da = initial(1)-final(1)
M = [da^3 da^4 da^5
3*da^2 4*da^3 5*da^4
6*da 12*da^2 20*da^3 ] ;
q = [initial(2)-final(2)
tan(theta1)
tan(phi1)/(bigL*(cos(theta1)^3))] ;
p = inv(M)*q ;
tau=(0:nbpt)'/nbpt
phi=tau.*tau.*(3-2*tau) ;
if orient=="reverse" then
a = (1-phi)*final(1) + phi*initial(1) ;
else
a = (1-phi)*initial(1) + phi*final(1) ;
end
da=a-final(1)
f= final(2)+ p(1).*da.^3 + p(2).*da.^4 + p(3).*da.^5 ;
df = 3*p(1).*da.^2 + 4*p(2).*da.^3 + 5*p(3).*da.^4 ;
ddf = 6*p(1).*da + 12*p(2).*da.^2 + 20*p(3).*da.^3 ;
k = ddf ./ ((1+df.*df).^(3/2)) ;
state=[ a f atan(df) atan(k*bigL)]
endfunction
function display_car_trajectory(state)
bigL=1
set figure_style new;clf();show_window()
a=gca()
drawlater()
a.isoview="on"
a.data_bounds=[min(state(:,1))-0.5*bigL, min(state(:,2))-1.5*bigL
max(state(:,1))+1.5*bigL, max(state(:,2))+1.5*bigL]
rect=matrix(a.data_bounds',-1,1)
xpoly(rect([1 3 3 1]),rect([2,2,4,4]),"lines",1)
C=build_car()
Cinit=[];Cend=[];Cinter=[];
for k=1:size(C,"*")
Cinit=[Cinit copy(C(k))];
Cinter=[Cinter,copy(C(k))];
Cend=[Cend,copy(C(k))]
end
// starting configuration
draw_car(Cinit,state(1,:))
// end configuration
draw_car(Cend,state($,:))
// intermediate configuration (inversion of velocity)
draw_car(Cinter,state(ceil(size(state,1)/2),:)) ;
// trajectory of the linearizing output
t1=polyline([state(1,1) state(1,2);state(1,1) state(1,2)]) ;
t1.line_style=2;
realtimeinit(0.1)
for i=1:size(state,1)
realtime(i)
drawlater()
draw_car(C, state(i,:))
t1.data=[t1.data;state(i,1) state(i,2)];
drawnow()
end
for i=(1:30)+size(state,1),realtime(i),end
endfunction
function C=build_car()
//build the graphic object for the car
//
//the car
hcar=polyline([-2,7,8,8,7,-2,-2;-2,-2,-1,1,2,2,-2]'/6)
hcar.foreground=2
// rear wheels
hwheel1=polyline([[-1 1]/8; [1 1]/6]')
hwheel1.thickness=2
hwheel2=polyline([[-1 1]/8; -[1 1]/6]')
hwheel2.thickness=2
// front wheels
hwheel3=polyline([[7 9]/8;[1 1]/6]')
hwheel3.thickness=2
hwheel4=polyline([[7 9]/8;-[1 1]/6]')
hwheel4.thickness=2
//return vector of handle on the objects
C=[hcar,hwheel1,hwheel2,hwheel3,hwheel4]
endfunction
function draw_car(C,pos)
if is_handle_valid(C) then
drawlater()
[x,y,theta,phi]=(pos(1),pos(2),pos(3),pos(4))
bigL=1
Rc=[cos(theta) sin(theta);-sin(theta) cos(theta)]
// the car
xy = [-2,-2;7,-2;8,-1;8,1;7,2;-2,2;-2,-2]/6
C(1).data=ones(xy)*diag([x;y])+bigL*xy*Rc
// rear wheels
xy=[[-1 1]/8; [1 1]/6]'
C(2).data=ones(xy)*diag([x;y])+bigL*xy*Rc
xy=[[-1 1]/8; -[1 1]/6]'
C(3).data=ones(xy)*diag([x;y])+bigL*xy*Rc
// front wheels
xy=[(1-cos(phi)/8) (1/6-sin(phi)/8)
(1+cos(phi)/8) (1/6+sin(phi)/8)]
C(4).data=ones(xy)*diag([x;y])+bigL*xy*Rc
xy=[(1-cos(phi)/8) (-1/6-sin(phi)/8)
(1+cos(phi)/8) (-1/6+sin(phi)/8)]
C(5).data=ones(xy)*diag([x;y])+bigL*xy*Rc
drawnow()
end
endfunction
function h=polyline(xy)
xpoly(xy(:,1),xy(:,2),"lines")
h=gce()
endfunction
|
