8d5cebd692b91b6589d39284e97f34503b6f902d
[scilab.git] / scilab / modules / cacsd / demos / flat / car.sci
1 function state=car_solve(initial,final)
2 //
3 //  CAR PACKING VIA FLATNESS AND FRENET FORMULAS
4 //
5 //    explicit computation and visualisation of the motions.
6 //
7 //    February 1993
8 //
9 // ............................................................
10 // :         pierre ROUCHON  <rouchon@cas.ensmp.fr>           :
11 // : Centre Automatique et Systemes, Ecole des Mines de Paris :
12 // : 60, bd Saint Michel -- 75272 PARIS CEDEX 06, France      :
13 // :    Telephone: (1) 40 51 91 15 --- Fax: (1) 43 54 18 93   :
14 // :..........................................................:
15 //
16 // initial: initial position [x,y,theta,phi]
17 // final  :   final position [x,y,theta,phi]
18 //        theta  : the car angle
19 //        phi    : the front wheel angle
20   
21   bigT = 1 ;//basic time interval for one  smooth motion (s)
22   bigL = 1 ;// car length (m) 
23
24   // computation of  intermediate configuration 
25   x0 = max(initial(1),final(2))   ....
26        + bigL*abs(tan(initial(3))) ...
27        + bigL*abs(tan(final(3))) ...
28        + bigL*(abs(initial(2)-final(2))/bigL)^(1/2) ;
29   y0 = (initial(2)+final(2))/2 ;
30   intermediate=[x0,y0,0,0]'
31
32   // first polynomial curve
33   state=[matrix(initial,1,-1);
34          car_polynomial_curve(initial,intermediate,"direct")]
35   //
36   // second polynomial curve
37   state = [ state; 
38             matrix(intermediate,1,-1) 
39             car_polynomial_curve(final,intermediate,"reverse")
40             matrix(final,1,-1)] 
41 endfunction
42
43 function state=car_polynomial_curve(initial,final,orient)
44
45   nbpt = 40 ; //  sampling of motion 
46   theta1 = initial(3) ; phi1 = initial(4) ;
47   da = initial(1)-final(1)
48   
49   M = [da^3     da^4     da^5 
50        3*da^2   4*da^3   5*da^4
51        6*da    12*da^2  20*da^3 ] ;
52   
53   q = [initial(2)-final(2)
54        tan(theta1) 
55        tan(phi1)/(bigL*(cos(theta1)^3))] ;
56   
57   p = inv(M)*q ;
58   tau=(0:nbpt)'/nbpt
59   phi=tau.*tau.*(3-2*tau) ;
60   if orient=='reverse' then
61     a = (1-phi)*final(1) + phi*initial(1) ;
62   else
63     a = (1-phi)*initial(1) + phi*final(1) ;
64   end
65   da=a-final(1)
66   
67   f=  final(2)+ p(1).*da^3 +    p(2).*da^4 +    p(3).*da^5 ;
68   df  =       3*p(1).*da^2 +  4*p(2).*da^3 +  5*p(3).*da^4 ;
69   ddf =       6*p(1).*da   + 12*p(2).*da^2 + 20*p(3).*da^3 ;
70   
71   k = ddf ./ ((1+df.*df)^(3/2)) ;
72   state=[ a   f  atan(df) atan(k*bigL)]
73 endfunction
74
75 function display_car_trajectory(state)
76   bigL=1
77   set figure_style new;clf();show_window()
78   a=gca()
79   drawlater()
80   a.isoview="on"
81   a.data_bounds=[min(state(:,1))-0.5*bigL, min(state(:,2))-1.5*bigL
82                  max(state(:,1))+1.5*bigL, max(state(:,2))+1.5*bigL]
83   rect=matrix(a.data_bounds',-1,1)
84   xpoly(rect([1 3 3 1]),rect([2,2,4,4]),'lines',1)
85   C=build_car()
86   Cinit=[];Cend=[];Cinter=[];
87   for k=1:size(C,'*')
88     Cinit=[Cinit copy(C(k))];
89     Cinter=[Cinter,copy(C(k))];
90     Cend=[Cend,copy(C(k))]
91   end
92   // starting configuration
93   draw_car(Cinit,state(1,:))
94   // end configuration
95   draw_car(Cend,state($,:))
96   // intermediate configuration (inversion of velocity)
97   draw_car(Cinter,state(ceil(size(state,1)/2),:)) ;
98   // trajectory of the linearizing output
99   t1=polyline([state(1,1) state(1,2);state(1,1) state(1,2)]) ;
100   t1.line_style=2;
101   realtimeinit(0.1)
102   for i=1:size(state,1)
103     realtime(i) 
104     drawlater()
105     draw_car(C, state(i,:)) 
106     t1.data=[t1.data;state(i,1) state(i,2)];
107     drawnow()
108   end 
109   for i=(1:30)+size(state,1),realtime(i),end
110 endfunction
111
112
113 function C=build_car()
114 //build the graphic object for the car
115 // 
116   //the car
117   hcar=polyline([-2,7,8,8,7,-2,-2;-2,-2,-1,1,2,2,-2]'/6)
118   hcar.foreground=2
119
120   // rear wheels
121   hwheel1=polyline([[-1 1]/8; [1 1]/6]')
122   hwheel1.thickness=2
123    
124   hwheel2=polyline([[-1 1]/8; -[1 1]/6]')
125   hwheel2.thickness=2
126   
127   // front wheels
128   hwheel3=polyline([[7 9]/8;[1 1]/6]')
129   hwheel3.thickness=2
130   hwheel4=polyline([[7 9]/8;-[1 1]/6]') 
131   hwheel4.thickness=2
132   //return vector of handle on the objects
133   C=[hcar,hwheel1,hwheel2,hwheel3,hwheel4]
134 endfunction
135
136 function draw_car(C,pos)
137   if is_handle_valid(C) then
138     drawlater()
139     [x,y,theta,phi]=(pos(1),pos(2),pos(3),pos(4))
140     bigL=1
141     Rc=[cos(theta) sin(theta);-sin(theta) cos(theta)]
142     // the car
143     xy = [-2,-2;7,-2;8,-1;8,1;7,2;-2,2;-2,-2]/6
144     C(1).data=ones(xy)*diag([x;y])+bigL*xy*Rc
145     // rear wheels
146     xy=[[-1 1]/8; [1 1]/6]'
147     C(2).data=ones(xy)*diag([x;y])+bigL*xy*Rc
148     xy=[[-1 1]/8; -[1 1]/6]'
149     C(3).data=ones(xy)*diag([x;y])+bigL*xy*Rc
150     // front wheels
151     xy=[(1-cos(phi)/8) (1/6-sin(phi)/8)
152       (1+cos(phi)/8) (1/6+sin(phi)/8)]
153     C(4).data=ones(xy)*diag([x;y])+bigL*xy*Rc
154     xy=[(1-cos(phi)/8) (-1/6-sin(phi)/8)
155       (1+cos(phi)/8) (-1/6+sin(phi)/8)]
156     C(5).data=ones(xy)*diag([x;y])+bigL*xy*Rc
157     drawnow()
158     show_pixmap();
159   end
160 endfunction
161
162 function h=polyline(xy)
163   xpoly(xy(:,1),xy(:,2),'lines')
164   h=gce()
165 endfunction