global bound rng l1 l2 ox2 oy2 para
l1=1;l2=1;

hh=findobj(gcf,'tag','x1');
xs=str2num(get(hh,'string'));
hh=findobj(gcf,'tag','y1');
ys=str2num(get(hh,'string'));
hh=findobj(gcf,'tag','x2');
xg=str2num(get(hh,'string'));
hh=findobj(gcf,'tag','y2');
yg=str2num(get(hh,'string'));

hh=findobj(gcf,'tag','obx');
obx=str2mat(get(hh,'string'));
hh=findobj(gcf,'tag','oby');
oby=str2mat(get(hh,'string'));
shx=eval(obx);shy=eval(oby);
rx=.35;ry=0.35;
l=linspace(0,2*pi,20);
xv=rx*cos(l)'+shx;yv=ry*sin(l)'+shy;
ox2=[xv;xv(1)];oy2=[yv;yv(1)];

qs=invkin(xs,ys);
qg=invkin(xg,yg);
para=[qs,qg ];
%GA parameters
pops=40;
crossprop=0.8;
mutprop=0.08;
maxgen=80;
bound=[-pi    pi;
       -pi    pi;
       -pi/4  pi/4;
       -pi/4  pi/4;
       0      6;
       0      6];
   
%initialization
numvar=size(bound,1);
rng=(bound(:,2)-bound(:,1))';
pop=zeros(pops,numvar);
%pop = initial population
pop(:,1:numvar)=(ones(pops,1)*rng).*(rand(pops,numvar))+...
    (ones(pops,1)*bound(:,1)');
tic
for it=1:maxgen
    fpop=fitnesstra2ob(pop,para);
    [cs,inds]=max(fpop);bchrom=pop(inds,:);
   % tournament selection
    toursize=5;
    players=ceil(pops*rand(pops,toursize));
    scores=fpop(players);
    [a,m]=max(scores');
    pind=zeros(1,pops);
    for ii=1:pops
        pind(ii)=players(ii,m(ii));
        parent(ii,:)=pop(pind(ii),:);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%crossover%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    offs=cross_singlepoint(parent,crossprop);   
    moffs=mutate1(offs,mutprop);
    pop=moffs;
    mm=fitnesstra2ob(pop,para);
    maxf(it)=max(fpop);    
    [bfit,bind]=max(mm);
    bsol=pop(bind,:);
    rec=recor2(bsol,para);
    trec(it)=rec(1);
    qrec(it)=rec(2);
    drec(it)=rec(3);
    torrec(it)=rec(4);
    if mm(inds) < cs
    pop(inds,:)=bchrom;
    end
    %*************************plot results
    qs1=para(1);qs2=para(2);
    qm1=bsol(1);qm2=bsol(2);
    qg1=para(3);qg2=para(4);
    
    [xs1,ys1]=pol2cart(qs1,l1);
    [xs2,ys2]=pol2cart(qs2+qs1,l2);
    xxs1=linspace(0,xs1);
    yys1=linspace(0,ys1);
    xxs2=linspace(0,xs2);xxs2=xxs2+xs1;
    yys2=linspace(0,ys2);yys2=yys2+ys1;
    
    [xm1,ym1]=pol2cart(qm1,l1);
    [xm2,ym2]=pol2cart(qm2+qm1,l2);
    xxm1=linspace(0,xm1);
    yym1=linspace(0,ym1);
    xxm2=linspace(0,xm2);xxm2=xxm2+xm1;
    yym2=linspace(0,ym2);yym2=yym2+ym1;
    
    [xg1,yg1]=pol2cart(qg1,l1);
    [xg2,yg2]=pol2cart(qg2+qg1,l2);
    xxg1=linspace(0,xg1);
    yyg1=linspace(0,yg1);
    xxg2=linspace(0,xg2);xxg2=xxg2+xg1;
    yyg2=linspace(0,yg2);yyg2=yyg2+yg1;
    xs=[xxs1,xxs2];ys=[yys1,yys2];
    xm=[xxm1,xxm2];ym=[yym1,yym2];
    xg=[xxg1,xxg2];yg=[yyg1,yyg2];
    kk=trajt(para,bsol);
    bq=kk(1:2,[5,7,10,13,15,17,20,23,25,27,30,33,35]);
    pq=kk(1:2,:);
    pcart=forkin(pq);
    px=pcart(1,:);py=pcart(2,:);
    
    if it==maxgen
      [xxt,yyt]=angls2links2(bq); 
      figure,plot(xxt',yyt')
      xlabel('x(m)')
      ylabel('y(m)')
      hold on
    end
    plot(px,py,ox2,oy2,'r')
    hold on
    axis([-2.5 2.5 -2.5 2.5])
    text(1.75,2.4,['gen. No.',num2str(it)]);
    plot(xs,ys,xm,ym,xg,yg)
    hold off
    pause(0)
end
toc
e=[1:maxgen];
figure,plot(e,1./maxf)
xlabel('generation')
ylabel('min. fitness')
tt=torque(kk(1:6,:));
t1=bsol(5);ti1=linspace(0,t1,20);
ti2=bsol(6);ti2=linspace(t1,ti2+t1,20);
time=[ti1 ti2];
q1=kk(1,:);q2=kk(2,:);
figure,plot(time,q1,'r--',time,q2,'g--+',time(20),q1(20),'ko',time(20),q2(20),'ko')
xlabel('Time(s)')
ylabel('joint angle(rad)')
h = legend('joint 1','joint 2',1);
v1=kk(3,:);v2=kk(4,:);
figure,plot(time,v1,'r--',time,v2,'g--+',time(20),v1(20),'ko',time(20),v2(20),'ko')
xlabel('Time(s)')
ylabel('joint velocity(rad/s)')
h = legend('joint 1','joint 2',1);

a1=kk(5,:);a2=kk(6,:);
figure,plot(time,a1,'r--',time,a2,'g--+',time(20),a1(20),'ko',time(20),a2(20),'ko')
xlabel('Time(s)')
ylabel('joint acceleration(rad/s^2)')

h = legend('joint 1','joint 2',1);
figure,plot(time,tt(1,:),'r--',time,tt(2,:),'g--+',time(20),tt(1,20),'ko',time(20),tt(2,20),'ko')
xlabel('Time(s)')
ylabel('joint tourque(N.m)')
h = legend('joint 1','joint 2',1);

figure,plot(e,trec)
xlabel('generation')
ylabel('consumed time for point to point motion(s)')
figure,plot(e,qrec)
xlabel('generation')
ylabel('total joint distance(rad)')
figure,plot(e,drec)
xlabel('generation')
ylabel('total cartesian trajectory length(m)')
figure,plot(e,torrec)
xlabel('generation')
ylabel('total excessive torque(N.m)')
bsol'