12abeb534565c91625e5732475bd09f023fb899c
[scilab.git] / scilab / modules / cacsd / demos / flat / truck.sci
1
2
3 function state=truck_solve(initial,final)
4 //
5 //  CAR  WITH 2 TRAILERS PACKING VIA FLATNESS AND FRENET FORMULAS
6 //
7 //    explicit computation and visualisation of the motions.
8 //
9 //    February 1993
10 //
11 // ............................................................
12 // :         pierre ROUCHON  <rouchon@cas.ensmp.fr>           :
13 // : Centre Automatique et Systemes, Ecole des Mines de Paris :
14 // : 60, bd Saint Michel -- 75272 PARIS CEDEX 06, France      :
15 // :    Telephone: (1) 40 51 91 15 --- Fax: (1) 43 54 18 93   :
16 // :..........................................................:
17 //
18 // initial: initial position [x,y,theta1,theta2,theta3,phi]
19 // final  :   final position [x,y,theta1,theta2,theta3,phi]
20 //        theta1,theta2,theta3  : the car and the trailers angles
21 //        phi    : the front wheel angle
22   
23   bigT = 1 ;//basic time interval for one  smooth motion (s)
24   bigL = 1 ;// car length (m) 
25   d1 = 1.5 ; d2 = 1 ; //trailers length
26
27   // computation of  intermediate configuration 
28   LL=bigL+d1+d2
29   x0 = max(initial(1),final(2))   ....
30        + LL*abs(tan(initial(3))) ...
31        + LL*abs(tan(initial(4))) ...
32        + LL*abs(tan(initial(5))) ...
33        + LL*(abs(initial(2)-final(2))/(d1+d2+bigL))^(1/2) ;
34   y0 = (initial(2)+final(2))/2 ;
35   intermediate=[x0,y0,0,0,0,0]'
36
37   // first polynomial curve
38   state=truck_polynomial_curve(initial,intermediate,"direct")
39   //
40   // second polynomial curve
41   state = [ state;
42             truck_polynomial_curve(final,intermediate,"reverse")            ]
43            
44 endfunction
45
46
47
48 function state=truck_polynomial_curve(initial,final,orient)
49
50   nbpt = 40 ; //  sampling of motion 
51   phi = initial(6) ;
52
53   theta2 = initial(3) ; 
54   theta1 = initial(4)+theta2
55   theta0 = initial(5)+theta1
56   
57   x0=initial(1)+d2*cos(theta2)+d1*cos(theta1) ;
58   y0=initial(2)+d2*sin(theta2)+d1*sin(theta1) ;
59   if orient<>'reverse' then
60     state = [x0 y0 theta0 theta1 theta2 phi] ;
61   else
62     state=[]
63   end
64   a0=final(1);a1=initial(1);b0=final(2)
65   db = initial(2)-final(2)
66   p = cr2Tkf(db,initial(3),initial(4),initial(5),phi) ;
67
68   tau=(0:nbpt)'/nbpt
69   phi=tau.*tau.*(3-2*tau) ;
70   if orient=='reverse' then
71     aa = (1-phi)*final(1) + phi*initial(1) ;
72   else
73     aa = (1-phi)*initial(1) + phi*final(1) ;
74   end
75   for i=1:(nbpt+1)
76     [bb,df,d2f,d3f,d4f,d5f] = cr2Tfjt(aa(i)) ;
77     [k2,k1,k0,dk0]=cr2Tfk(df,d2f,d3f,d4f,d5f) ;
78     theta2 = atan(df);
79     theta1 = atan(k2*d2)+theta2;
80     theta0 = atan(k1*d1) + theta1 ;
81     phi = atan(k0*bigL) ;
82     x0=aa(i)+d2*cos(theta2)+d1*cos(theta1) ;
83     y0=bb+d2*sin(theta2)+d1*sin(theta1) ;
84     state= [ state; 
85              x0 y0 theta0 theta1 theta2 phi] ;
86   end
87 endfunction
88
89 function [ff,df,d2f,d3f,d4f,d5f]=cr2Tfjt(a)
90 //
91 //
92   da = a-a0
93   M= [  da^5      da^6      da^7      da^8       da^9 
94       5*da^4    6*da^5    7*da^6    8*da^7     9*da^8 
95      20*da^3   30*da^4   42*da^5   56*da^6    72*da^7 
96      60*da^2  120*da^3  210*da^4  336*da^5   504*da^6 
97     120*da^1  360*da^2  840*da^3 1680*da^4  3024*da^5 
98     120       720*da^1 2520*da^2 6720*da^3 15120*da^4]*p ;
99   ff  = M(1) + b0 ;
100   df  = M(2) ;
101   d2f = M(3) ;
102   d3f = M(4) ;
103   d4f = M(5) ;
104   d5f = M(6) ;
105 endfunction
106
107 function coef=cr2Tkf(b,theta2,theta12,theta01,phi)
108 //
109 //
110   da = a1-a0
111 M = [1*da^5    1*da^6   1*da^7    1*da^8    1*da^9 
112      5*da^4    6*da^5   7*da^6    8*da^7    9*da^8 
113     20*da^3   30*da^4  42*da^5   56*da^6   72*da^7 
114     60*da^2  120*da^3 210*da^4  336*da^5  504*da^6 
115    120*da^1  360*da^2 840*da^3 1680*da^4 3024*da^5] ;
116 //
117 //
118 df = tan(theta2) ;
119 //
120 // curvatures
121 k2=tan(theta12)/d2;k1=tan(theta01)/d1;k0=tan(phi)/bigL;
122 // 
123 ddf = k2*((1+df*df)^(3/2)) ;
124 //
125 // derivative of k2
126 dk2ds2 = ( (1+(d2*k2)^2)/d2 )*( (1+(d2*k2)^2)^(1/2)*k1 - k2 ) ;
127 dk2 = (1+df*df)^(1/2) * dk2ds2 ;
128 //
129 dddf = dk2 * (1+df*df)^(3/2)  + 3*k2*df*ddf*(1+df*df)^(1/2) ;
130 //
131 // second derivative of k2
132 dk1ds1 = ( (1+(d1*k1)^2)/d1 )*( (1+(d1*k1)^2)^(1/2)*k0 - k1 ) ;
133 dk1ds2 = dk1ds1 *  (1+(d2*k2)^2)^(1/2) ;
134 //
135 ddk2ds2 =  ....
136   3*d2*k2*dk2ds2*(1+(d2*k2)^2)^(1/2)*k1 ....
137 + (1+(d2*k2)^2)^(3/2)*dk1ds2/d2 ...
138 - (1/d2+3*d2*k2*k2)*dk2ds2 ;
139 //
140 ddk2 = .... 
141    df*ddf*((1+df*df)^(-1/2))*dk2ds2 ....
142  + (1+df*df)*ddk2ds2 ; 
143 //
144 ddddf =  ....
145    ddk2 * (1+df*df)^(3/2) ....
146  + 6*dk2*df*ddf*(1+df*df)^(1/2) ....
147  + 3*k2*ddf*ddf*(1+df*df)^(1/2) ....
148  + 3*k2*df*dddf*(1+df*df)^(1/2) ....
149  + 3*k2*(df*ddf)^2*(1+df*df)^(-1/2) ;
150 //
151 //
152 //
153 //
154 coef =inv(M)* [b;df;ddf;dddf;ddddf] ;
155 endfunction
156
157 function [k2,k1,k0,dk0]=cr2Tfk(df,d2f,d3f,d4f,d5f)
158 //
159 //
160 // computation of curvatures from derivatives of b=f(a)
161 //
162 g = (1+df^2)^(-1/2);
163 dg = - df*d2f*g^3 ;
164 d2g = - g*g*(d2f^2*g+df*d3f*g+3*df*d2f*dg) ;
165 d3g = ....
166 - 2*g*dg*(d2f^2*g+df*d3f*g+3*df*d2f*dg) ....
167 - g^2*(3*d2f*d3f*g+df*d4f*g ....
168        + 4*d2f^2*dg+4*df*d3f*dg+3*df*d2f*d2g) ;
169 //
170 k2  = d2f * g^3  ;
171 dk2 = d3f*g^3 + 3*d2f*g^2*dg ;
172 d2k2 = g^2*(d4f*g+6*d3f*dg+3*d2f*d2g) + 6*g*dg^2*d2f ;
173 d3k2 = 2*g*dg*(d4f*g+6*d3f*dg+3*d2f*d2g) ....
174  + g^2*(d5f*g+7*d4f*dg+9*d3f*d2g+3*d2f*d3g) ....
175  +6*dg^3*d2f+12*g*dg*d2g*d2f+6*g*dg^2*d3f ;
176 //
177 g2 = (1+(d2*k2)^2)^(-1/2) ;
178 dg2 = -d2^2*k2*dk2*g2^3 ;
179 d2g2 = -d2^2*g2^2*(dk2^2*g2+k2*d2k2*g2+3*k2*dk2*dg2) ;
180 //
181 h2 = g2^3*g ;
182 dh2 = g2^2*(3*dg2*g+g2*dg);
183 d2h2 = 2*g2*dg2*(3*dg2*g+g2*dg) ....
184      + g2^2*(3*d2g2*g+4*dg2*dg+g2*d2g) ;
185 //
186 k1 = g2*k2 + d2*h2*dk2 ;
187 dk1 = dg2*k2 + g2*dk2 + d2 * (dh2*dk2+h2*d2k2) ;
188 d2k1 = d2g2*k2 + 2*dg2*dk2 + g2*d2k2 ....
189    + d2 * (d2h2*dk2+2*dh2*d2k2+h2*d3k2) ;
190 //
191 g1 = (1+(d1*k1)^2)^(-1/2) ;
192 dg1 = - d1^2*k1*dk1*g1^3 ;
193 //
194 k0 = g1*k1 + d1*g1^3*g2*g*dk1 ;
195 dk0 = dg1*k1 + g1*dk1 ....
196  + d1*g1^2*(3*dg1*g2*g*dk1+g1*dg2*g*dk1 ....
197           +  g1*g2*dg*dk1+g1*g2*g*d2k1) ;
198
199 endfunction
200
201 function display_truck_trajectory(state)
202   bigL = 1 ; d1 = 1.5 ; d2 = 1;
203   a=gca();
204   drawlater();
205   a.isoview="on"
206   a.data_bounds=[min(state(:,1))-1.5*(d1+d2), min(state(:,2))-bigL
207                  max(state(:,1))+1.5*bigL, max(state(:,2))+bigL]
208   rect=matrix(a.data_bounds',-1,1)
209   xpoly(rect([1 3 3 1]),rect([2,2,4,4]),'lines',1)
210   C=build_truck()
211   Cinit=[];
212   Cend=[];
213   Cinter=[];
214   for k=1:size(C,'*')
215     Cinit=[Cinit copy(C(k))];
216     Cinter=[Cinter,copy(C(k))];
217     Cend=[Cend,copy(C(k))]
218   end
219   // starting configuration
220   draw_truck(Cinit,state(1,:))
221   // end configuration
222   draw_truck(Cend,state($,:))
223   // intermediate configuration (inversion of velocity)
224   draw_truck(Cinter,state(ceil(size(state,1)/2),:)) ;
225   // trajectory of the linearizing output
226   x_lin = state(:,1)-d1*cos(state(:,4))-d2*cos(state(:,5)) ;
227   y_lin = state(:,2)-d1*sin(state(:,4))-d2*sin(state(:,5)) ;
228
229   t1=polyline([x_lin(1) y_lin(1);x_lin(1) y_lin(1)]) ;
230   t2=polyline([state(1,1) state(1,2);state(1,1) state(1,2)]) ;
231   
232   t1.line_style=2;
233   t2.line_style=2;t2.foreground=5
234   realtimeinit(0.2)
235   for i=1:size(state,1)
236     realtime(i) 
237     drawlater()
238     draw_truck(C, state(i,:)) 
239     t1.data=[t1.data;x_lin(i), y_lin(i)];
240     t2.data=[t2.data;state(i,1), state(i,2)];
241     drawnow()
242   end 
243   for i=(1:30)+size(state,1),realtime(i),end
244 endfunction
245
246
247 function C=build_truck()
248 //build the graphic object for the truck
249 // 
250   //the car
251   hcar=polyline([-2,7,8,8,7,-2,-2;-2,-2,-1,1,2,2,-2]'/6)
252   hcar.foreground=2
253   // rear wheels
254   hwheel1=polyline([[-1 1]/8; [1 1]/6]')
255   hwheel1.thickness=2
256    
257   hwheel2=polyline([[-1 1]/8; -[1 1]/6]')
258   hwheel2.thickness=2
259   
260   // front wheels
261   hwheel3=polyline([[7 9]/8;[1 1]/6]')
262   hwheel3.thickness=2
263   hwheel4=polyline([[7 9]/8;-[1 1]/6]') 
264   hwheel4.thickness=2
265   
266   //Trailer 1
267
268   ht1=polyline([-1,1,1,-1,-1;-1,-1,1,1,-1]'*bigL/3)
269   ht1.foreground=2
270   hwheel5=polyline([[-1 1]/8;[1 1]/6]'*bigL)
271   hwheel5.thickness=2
272   hwheel6=polyline([[-1 1]/8;-[1 1]/6]'*bigL)
273   hwheel6.thickness=2
274   hhitch1=polyline([bigL/3  d1;0 0]')
275   hhitch1.foreground=2
276   //Trailer 2
277   ht2=polyline([-1,1,1,-1,-1;-1,-1,1,1,-1]'*bigL/3)
278   ht2.foreground=2
279   hwheel7=polyline([[-1 1]/8;[1 1]/6]'*bigL)
280   hwheel7.thickness=2
281   hwheel8=polyline([[-1 1]/8;-[1 1]/6]'*bigL)
282   hwheel8.thickness=2
283   hhitch2=polyline([bigL/3  d2;0 0]')
284   hhitch2.foreground=2
285
286
287
288   //return vector of handle on the objects
289   C=[hcar,hwheel1,hwheel2,hwheel3,hwheel4,..
290      ht1,hwheel5, hwheel6,hhitch1,..
291      ht2, hwheel7,hwheel8,hhitch2]
292 endfunction
293
294 function draw_truck(C,pos)
295   drawlater()
296   [x,y,theta1,theta2,theta3,phi]=(pos(1),pos(2),pos(3),pos(4),pos(5),pos(6))
297   bigL = 1 ; d1 = 1.5 ; d2 = 1;
298   Rc=[cos(theta1) sin(theta1);-sin(theta1) cos(theta1)]
299   // the car
300   xy = [-2,-2;7,-2;8,-1;8,1;7,2;-2,2;-2,-2]/6
301   C(1).data=ones(xy)*diag([x;y])+bigL*xy*Rc
302   // rear wheels
303   xy=[[-1 1]/8; [1 1]/6]'
304   C(2).data=ones(xy)*diag([x;y])+bigL*xy*Rc
305   xy=[[-1 1]/8; -[1 1]/6]'
306   C(3).data=ones(xy)*diag([x;y])+bigL*xy*Rc
307   // front wheels
308   xy=[(1-cos(phi)/8) (1/6-sin(phi)/8)
309       (1+cos(phi)/8) (1/6+sin(phi)/8)]
310   C(4).data=ones(xy)*diag([x;y])+bigL*xy*Rc
311   xy=[(1-cos(phi)/8) (-1/6-sin(phi)/8)
312       (1+cos(phi)/8) (-1/6+sin(phi)/8)]
313   C(5).data=ones(xy)*diag([x;y])+bigL*xy*Rc
314   
315   //Trailer 1
316   Rc=[cos(theta2) sin(theta2);-sin(theta2) cos(theta2)]
317   x = x - d1*cos(theta2) ;
318   y = y - d1*sin(theta2) ;
319   xy = [-1,1,1,-1,-1;-1,-1,1,1,-1]'*bigL/3;
320   C(6).data=ones(xy)*diag([x;y])+bigL*xy*Rc
321   //wheels
322   xy=[[-1 1]/8; [1 1]/6]'
323   C(7).data=ones(xy)*diag([x;y])+bigL*xy*Rc
324   xy=[[-1 1]/8; -[1 1]/6]'
325   C(8).data=ones(xy)*diag([x;y])+bigL*xy*Rc
326   //hitch
327   xy=[bigL/3  d1;0 0]'
328   C(9).data=ones(xy)*diag([x;y])+bigL*xy*Rc
329
330   //Trailer 2
331   Rc=[cos(theta3) sin(theta3);-sin(theta3) cos(theta3)]
332   x = x - d2*cos(theta3) ;
333   y = y - d2*sin(theta3) ;
334   xy = [-1,1,1,-1,-1;-1,-1,1,1,-1]'*bigL/3;
335   C(10).data=ones(xy)*diag([x;y])+bigL*xy*Rc
336   //wheels
337   xy=[[-1 1]/8; [1 1]/6]'
338   C(11).data=ones(xy)*diag([x;y])+bigL*xy*Rc
339   xy=[[-1 1]/8; -[1 1]/6]'
340   C(12).data=ones(xy)*diag([x;y])+bigL*xy*Rc
341   //hitch
342   xy=[bigL/3  d2;0 0]'
343   C(13).data=ones(xy)*diag([x;y])+bigL*xy*Rc
344   drawnow()
345   show_pixmap();
346 endfunction
347
348 function h=polyline(xy)
349   xpoly(xy(:,1),xy(:,2),'lines')
350   h=gce()
351 endfunction