Update demos system
[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 = maxi(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=[mini(state(:,1))-1.5*(d1+d2), mini(state(:,2))-bigL
207                  maxi(state(:,1))+1.5*bigL, maxi(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=[];Cend=[];Cinter=[];
212   for k=1:size(C,'*')
213     Cinit=[Cinit copy(C(k))];
214     Cinter=[Cinter,copy(C(k))];
215     Cend=[Cend,copy(C(k))]
216   end
217   // starting configuration
218   draw_truck(Cinit,state(1,:))
219   // end configuration
220   draw_truck(Cend,state($,:))
221   // intermediate configuration (inversion of velocity)
222   draw_truck(Cinter,state(ceil(size(state,1)/2),:)) ;
223   // trajectory of the linearizing output
224   x_lin = state(:,1)-d1*cos(state(:,4))-d2*cos(state(:,5)) ;
225   y_lin = state(:,2)-d1*sin(state(:,4))-d2*sin(state(:,5)) ;
226
227   t1=polyline([x_lin(1) y_lin(1);x_lin(1) y_lin(1)]) ;
228   t2=polyline([state(1,1) state(1,2);state(1,1) state(1,2)]) ;
229   
230   t1.line_style=2;
231   t2.line_style=2;t2.foreground=5
232   realtimeinit(0.2)
233   for i=1:size(state,1)
234     realtime(i) 
235     drawlater()
236     draw_truck(C, state(i,:)) 
237     t1.data=[t1.data;x_lin(i), y_lin(i)];
238     t2.data=[t2.data;state(i,1), state(i,2)];
239     drawnow()
240   end 
241   for i=(1:30)+size(state,1),realtime(i),end
242   xdel()
243 endfunction
244
245
246 function C=build_truck()
247 //build the graphic object for the truck
248 // 
249   //the car
250   hcar=polyline([-2,7,8,8,7,-2,-2;-2,-2,-1,1,2,2,-2]'/6)
251   hcar.foreground=2
252   // rear wheels
253   hwheel1=polyline([[-1 1]/8; [1 1]/6]')
254   hwheel1.thickness=2
255    
256   hwheel2=polyline([[-1 1]/8; -[1 1]/6]')
257   hwheel2.thickness=2
258   
259   // front wheels
260   hwheel3=polyline([[7 9]/8;[1 1]/6]')
261   hwheel3.thickness=2
262   hwheel4=polyline([[7 9]/8;-[1 1]/6]') 
263   hwheel4.thickness=2
264   
265   //Trailer 1
266
267   ht1=polyline([-1,1,1,-1,-1;-1,-1,1,1,-1]'*bigL/3)
268   ht1.foreground=2
269   hwheel5=polyline([[-1 1]/8;[1 1]/6]'*bigL)
270   hwheel5.thickness=2
271   hwheel6=polyline([[-1 1]/8;-[1 1]/6]'*bigL)
272   hwheel6.thickness=2
273   hhitch1=polyline([bigL/3  d1;0 0]')
274   hhitch1.foreground=2
275   //Trailer 2
276   ht2=polyline([-1,1,1,-1,-1;-1,-1,1,1,-1]'*bigL/3)
277   ht2.foreground=2
278   hwheel7=polyline([[-1 1]/8;[1 1]/6]'*bigL)
279   hwheel7.thickness=2
280   hwheel8=polyline([[-1 1]/8;-[1 1]/6]'*bigL)
281   hwheel8.thickness=2
282   hhitch2=polyline([bigL/3  d2;0 0]')
283   hhitch2.foreground=2
284
285
286
287   //return vector of handle on the objects
288   C=[hcar,hwheel1,hwheel2,hwheel3,hwheel4,..
289      ht1,hwheel5, hwheel6,hhitch1,..
290      ht2, hwheel7,hwheel8,hhitch2]
291 endfunction
292
293 function draw_truck(C,pos)
294   drawlater()
295   [x,y,theta1,theta2,theta3,phi]=(pos(1),pos(2),pos(3),pos(4),pos(5),pos(6))
296   bigL = 1 ; d1 = 1.5 ; d2 = 1;
297   Rc=[cos(theta1) sin(theta1);-sin(theta1) cos(theta1)]
298   // the car
299   xy = [-2,-2;7,-2;8,-1;8,1;7,2;-2,2;-2,-2]/6
300   C(1).data=ones(xy)*diag([x;y])+bigL*xy*Rc
301   // rear wheels
302   xy=[[-1 1]/8; [1 1]/6]'
303   C(2).data=ones(xy)*diag([x;y])+bigL*xy*Rc
304   xy=[[-1 1]/8; -[1 1]/6]'
305   C(3).data=ones(xy)*diag([x;y])+bigL*xy*Rc
306   // front wheels
307   xy=[(1-cos(phi)/8) (1/6-sin(phi)/8)
308       (1+cos(phi)/8) (1/6+sin(phi)/8)]
309   C(4).data=ones(xy)*diag([x;y])+bigL*xy*Rc
310   xy=[(1-cos(phi)/8) (-1/6-sin(phi)/8)
311       (1+cos(phi)/8) (-1/6+sin(phi)/8)]
312   C(5).data=ones(xy)*diag([x;y])+bigL*xy*Rc
313   
314   //Trailer 1
315   Rc=[cos(theta2) sin(theta2);-sin(theta2) cos(theta2)]
316   x = x - d1*cos(theta2) ;
317   y = y - d1*sin(theta2) ;
318   xy = [-1,1,1,-1,-1;-1,-1,1,1,-1]'*bigL/3;
319   C(6).data=ones(xy)*diag([x;y])+bigL*xy*Rc
320   //wheels
321   xy=[[-1 1]/8; [1 1]/6]'
322   C(7).data=ones(xy)*diag([x;y])+bigL*xy*Rc
323   xy=[[-1 1]/8; -[1 1]/6]'
324   C(8).data=ones(xy)*diag([x;y])+bigL*xy*Rc
325   //hitch
326   xy=[bigL/3  d1;0 0]'
327   C(9).data=ones(xy)*diag([x;y])+bigL*xy*Rc
328
329   //Trailer 2
330   Rc=[cos(theta3) sin(theta3);-sin(theta3) cos(theta3)]
331   x = x - d2*cos(theta3) ;
332   y = y - d2*sin(theta3) ;
333   xy = [-1,1,1,-1,-1;-1,-1,1,1,-1]'*bigL/3;
334   C(10).data=ones(xy)*diag([x;y])+bigL*xy*Rc
335   //wheels
336   xy=[[-1 1]/8; [1 1]/6]'
337   C(11).data=ones(xy)*diag([x;y])+bigL*xy*Rc
338   xy=[[-1 1]/8; -[1 1]/6]'
339   C(12).data=ones(xy)*diag([x;y])+bigL*xy*Rc
340   //hitch
341   xy=[bigL/3  d2;0 0]'
342   C(13).data=ones(xy)*diag([x;y])+bigL*xy*Rc
343   drawnow()
344   show_pixmap();
345 endfunction
346
347 function h=polyline(xy)
348   xpoly(xy(:,1),xy(:,2),'lines')
349   h=gce()
350 endfunction