% Mouse-driven motion planning % for heavy chain system % march 99 % Ecole des Mines de Paris, % Centre Automatique et Systemes % 60 Bd St Michel % 75272 Paris % Nicolas Petit: petit@cas.ensmp.fr % Pierre Rouchon: rouchon@cas.ensmp.fr % % Linear approximation (Cf the model studied by Daniel Bernoulli 1738 ) % D_tt H = D_x ( g x D_x H ) = 0 % H(x=L,t)= D (the control, horizontal position of the trolley) % % flat output: y(t)=H(0,t) the position of the free end % motion planing formulae % % H(x,t) = 1/(2 pi) int_0^pi [ y(t - 2 sqrt(x/g) sin s) + y(t + 2 sqrt(x/g) sin s) ] ds % derived via symbolic calculus (Poisson integral representation of the % Bessel function J_0) % % clear all; L=1.; % rope length in m A_max= 1.5*L ; % amplitude of horizontal motion depth in m g=9.81; % garvity in m/s^2 T_max=2*sqrt(A_max/g); % interval of motion (s) DT=2*sqrt(L/g); scale=min(11., 11/max(A_max*14/19,L)); % space mesh dx=L/40.; x=[0:dx:L]; nx=length(x); H=zeros(1,nx); % time step dt0=DT/100; dt=dt0/4/2; dtmin=DT/1000; dtmax=DT/3/2; YY=0*[-DT:dt0:DT]; mt=length(YY); m0=(mt+1)/2.; mm=[1:mt-1]; Y=0*[-5*DT:dt:0]; D=Y;t=Y; nt=length(Y); nn=[1:nt-1]; figure(1),clf; %set(gcf,'renderer','openGL'); go=1; mouse=0; set(gca,'position',[0 0 1 1],'visible','off','Xlim',[0 19],'Ylim',[0 14].... ,'nextplot','add') set(gcf,'CloseRequestFcn','go=0; closereq;',... 'WindowButtonUpFcn',['mouse=0;set(ref,''facecolor'',''y'') ;'... 'set(gcf,''WindowButtonMotionFcn'','''');']); % trolley Xt=1+[0 1 1 0 ]+scale*H(nx); Yt=12+[0 0 1/2 1/2 ]; trol=fill(Xt,Yt,'g','EraseMode','background'); % cable Xc=1.5 + scale*H; Yc=12+scale*(x-1); chain=line(Xc,Yc,'EraseMode','background','LineWidth', 4, 'Color', [0 0 1 ]); % ref Xr=1+[0 1 0.5 0 ] + scale*H(nx); Yr=0.3+[0 0 1/2 0 ]; ref=fill(Xr,Yr,'y', 'EraseMode','xor',... 'ButtonDownFcn',['mouse=1; set(ref,''facecolor'',''r'');' ... 'set(gcf,''WindowButtonMotionFcn'',''1;'')']); Hplay=uicontrol('style','push',... 'units','normalized','position',[15/19 12.6/14 2/19 .5/14], .... 'string','Plot','callback', ... ['figure(2);clf;' ... 'subplot(212);' ... 'plot(t,Y,''linewidth'',3);'... 'title(''flat output (m) versus time'');'... 'subplot(211);'... 'plot(t,D,''linewidth'',3);'... 'title(''troley position (m/s)versus time''); pause;figure(1);'... ]); set(Hplay,'visible','off'); Hrun=uicontrol('style','push',... 'units','normalized','position',[1/19 12.7/14 2/19 .5/14], .... 'string','stop','callback', ... ' ; ' ,'visible','off'); % title text(9.5,13.5,'HEAVY CHAIN: MOUSE-DRIVEN MOTION PLANNING','color',[0 0 0],... 'EraseMode','background','HorizontalAlignment','center'); text(9.5,13,'display speed slider','color',[0 0 0],'EraseMode','background','HorizontalAlignment','center'); p1=[7.5/19 12.6/14 4/19 0.2/14]; Hslider=uicontrol(figure(1),'units','normalized','style','slider','position',p1,... 'min',log(dtmin),'max',log(dtmax),'value',log(dt0),... 'callback',['buf=get(Hslider,''value'');dt=exp(buf);']); figure(1); pause(1) set(Hrun,'string','Stop','callback','go=0;'); set(Hplay,'visible','on') set(Hrun,'visible','on') t1=T_max/5/5; t2=t1/2; seuil=A_max/5/10; y1=0; y2=0; dth=pi/100; th=[0:dth:pi-dth]; Sth=sin(th); tt=0.; y1_old=y1; buf(1,1)=1.5; while (go >0) if mouse > 0 buf=get(gca,'CurrentPoint'); end A=(buf(1,1)-1.5)/scale; A=max(0,A); A=min(A_max,A); y1=(y1+dt*y2/t1)/(1+dt/t1); t2b=t2*max(1,abs(A-y2)/seuil); y2=(y2+dt*A/t2b)/(1+dt/t2b); tt=tt+dt; if (tt >= dt0); ii=floor(tt/dt0); dy1=(y1-y1_old)/tt; for iii=1:ii; YY(mm)=YY(mm+1);YY(mt)=y1_old+iii*dt0*dy1; end; y1_old=y1-dy1*(tt-ii*dt0); tt=0.; end; for ix=1:nx; tau=2*sqrt(x(ix)/g); s=tau*Sth; ns=floor((mt-m0)*s/DT); H(ix)=(dth/2/pi)*sum(YY(m0+ns)+YY(m0-ns)); end Y(nn)=Y(nn+1); Y(nt)=H(1); D(nn)=D(nn+1); D(nt)=H(nx); t(nn)=t(nn+1); t(nt)=t(nt)+dt; Xt=1+[0 1 1 0 ] + scale*H(nx); Xr=1+[0 1 0.5 0 ] + scale*A; Xc=1.5 + scale*H; set(trol, 'XData',Xt); set(chain,'XData',Xc); set(ref,'XData',Xr); drawnow; end; close(figure(1)); close(figure(2)); return