CACSD demos: fix warnings
[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     end
159 endfunction
160
161 function h=polyline(xy)
162     xpoly(xy(:,1),xy(:,2),"lines")
163     h=gce()
164 endfunction