%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  运动学  非线性方程求解+动画   %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear all;

a1=15;  a2=28;  a3=32;  a4=30;  % 机构尺寸参数
rata0=0;  rata1=0.2;  rata2=0.5;  rata3=0.8;  rata4=1;

set(gcf,'Color',[1,1,1])

% 开始计算

gx(1)=-30;  gy(1)=0;  gx(2)=50;gy(2)=0;   % 地面
  
estimate_phi10=pi/4;
estimate_phi20=3*pi/2;
eps=1e-5;
NUM=361;

omega0=100*pi/180;

for ii=1:NUM 
    t(ii)=(ii-1)/100;
    theta(ii)=omega0*t(ii);
    estimate_phi1=estimate_phi10;
    estimate_phi2=estimate_phi20;

    %求转角
    for jj=1:100
       f1=a1*cos(theta(ii))+a2*cos(estimate_phi1)+a3*cos(estimate_phi2)-a4;
       f2=a1*sin(theta(ii))+a2*sin(estimate_phi1)+a3*sin(estimate_phi2);

       if(abs(f1)<eps && abs(f2)<eps)
          phi1(ii)=estimate_phi1;
          phi2(ii)=estimate_phi2;
            estimate_phi10=estimate_phi1;
            estimate_phi20=estimate_phi2;
            '--------ok----------';
            break;
       else
           J=[-a2*sin(estimate_phi1) -a3*sin(estimate_phi2);
               a2*cos(estimate_phi1) a3*cos(estimate_phi2)];
           ff(1)=f1;
           ff(2)=f2;
           
           dtheta=-inv(J)*ff';
           estimate_phi1=estimate_phi1+dtheta(1);
           estimate_phi2=estimate_phi2+dtheta(2);
           if(jj==100)
               '---------- solve theta error -------'
           end
       end
   end
end

% 求各点的位置
for ii=1:NUM 
    x(ii,1)=0;
    y(ii,1)=0;
    x(ii,2)=0;
    y(ii,2)=1;
    x(ii,3)=x(ii,2)+a1*cos(theta(ii));
    y(ii,3)=y(ii,2)+a1*sin(theta(ii));
    x(ii,4)=x(ii,3)+a2*cos(phi1(ii));
    y(ii,4)=y(ii,3)+a2*sin(phi1(ii));
    x(ii,5)=a4;
    y(ii,5)=1;
    x(ii,6)=a4;
    y(ii,6)=0;
    
    xp0(ii)=x(ii,3)*rata0+x(ii,4)*(1-rata0);
    yp0(ii)=y(ii,3)*rata0+y(ii,4)*(1-rata0);
    xp1(ii)=x(ii,3)*rata1+x(ii,4)*(1-rata1);
    yp1(ii)=y(ii,3)*rata1+y(ii,4)*(1-rata1);
    xp2(ii)=x(ii,3)*rata2+x(ii,4)*(1-rata2);
    yp2(ii)=y(ii,3)*rata2+y(ii,4)*(1-rata2);
    xp3(ii)=x(ii,3)*rata3+x(ii,4)*(1-rata3);
    yp3(ii)=y(ii,3)*rata3+y(ii,4)*(1-rata3);
    xp4(ii)=x(ii,3)*rata4+x(ii,4)*(1-rata4);
    yp4(ii)=y(ii,3)*rata4+y(ii,4)*(1-rata4);    
end

% 求角速度
for ii=1:NUM 
    A=[-a2*sin(phi1(ii)) -a3*sin(phi2(ii));
        a2*cos(phi1(ii)) a3*cos(phi2(ii))];
    B=[a1*omega0*sin(theta(ii));-a1*omega0*cos(theta(ii))];
    X=inv(A)*B;
    
    dphi1_dt(ii)=X(1);
    dphi2_dt(ii)=X(2);
end

% 求角加速度
for ii=1:NUM 
    A=[-a2*sin(phi1(ii)) -a3*sin(phi2(ii));
        a2*cos(phi1(ii)) a3*cos(phi2(ii))];
    B=[a2*(dphi1_dt(ii))^2*cos(phi1(ii))+a3*(dphi2_dt(ii))^2*cos(phi2(ii))+a1*omega0^2*cos(theta(ii));
       a2*(dphi1_dt(ii))^2*sin(phi1(ii))+a3*(dphi2_dt(ii))^2*sin(phi2(ii))+a1*omega0^2*sin(theta(ii))];
    X=inv(A)*B;
    
    ddphi1_dt2(ii)=X(1);
    ddphi2_dt2(ii)=X(2);
end

figure(1)
plot(t,theta,'k:',t,phi1,'k--',t,phi2,'k-');
xlabel('t/s','FontSize',9);
ylabel('角度/rad','FontSize',9);
legend({'θ','φ_1','φ_2'},'FontSize',8) ;
set(gcf, 'Units', 'centimeters', 'Position', [0, 0, 7, 5]);
set(gca, 'FontName', 'Times New Roman', 'FontSize', 8)
print('Example_D_5_Figure_1.eps','-depsc')

figure(2)
plot(theta,phi1,'k--',theta,phi2,'k-');
xlabel('\theta/rad','FontSize',9);
ylabel('\phi_1 and \phi_2 /rad','FontSize',9);
legend({'φ1','φ2'},'FontSize',8) ;
axis('equal')

figure(3)
plot(theta,omega0+0*dphi1_dt,'k--',theta,dphi1_dt,'k--',theta,dphi2_dt,'k-');
xlabel('\theta/rad');
% ylabel('d\theta/dt and d\phi_1/dt and d\phi_2/dt /rad/s');
ylabel('角速度/rad/s');
legend('dθ/dt','dφ_1/dt','dφ_2/dt') ;

figure(5)
plot(theta,0*ddphi1_dt2,'k:',theta,ddphi1_dt2,'k--',theta,ddphi2_dt2,'k-');
xlabel('\theta/rad');
ylabel('角加速度/rad/s^2');
legend('d^2θ/dt^2','d^2φ_1/dt^2','d^2φ_2/dt^2') ;

figure(6)
% 地面
plot(gx,gy,'k','LineWidth',1);  hold on;
plot(xp0,yp0,'c-.','LineWidth',1);  hold on;
plot(xp1,yp1,'r-.','LineWidth',1);  hold on;
plot(xp2,yp2,'b-.','LineWidth',1);  hold on;
plot(xp3,yp3,'g-.','LineWidth',1);  hold on;
plot(xp4,yp4,'c-.','LineWidth',1);  hold on;
axis off;
h1=line('Color','w','LineStyle','-','LineWidth',2,'Marker','o','Markersize',3);
h2=line('Color','r','LineStyle','-','LineWidth',1,'Marker','*','Markersize',4);
h3=line('Color','r','LineStyle','-','LineWidth',1,'Marker','*','Markersize',4);
h4=line('Color','r','LineStyle','-','LineWidth',1,'Marker','*','Markersize',4);
h5=line('Color','r','LineStyle','-','LineWidth',1,'Marker','*','Markersize',4);
h6=line('Color','r','LineStyle','-','LineWidth',1,'Marker','*','Markersize',4);

for ii=1:NUM  
    set(h1,'XData',x(ii,:),'YData',y(ii,:));
    set(h2,'XData',xp0(ii),'YData',yp0(ii));
    set(h3,'XData',xp1(ii),'YData',yp1(ii));
    set(h4,'XData',xp2(ii),'YData',yp2(ii));
    set(h5,'XData',xp3(ii),'YData',yp3(ii));
    set(h6,'XData',xp4(ii),'YData',yp4(ii));
    axis([-30,40,-20,40]);axis equal

    drawnow;
    pause(0.05);
end
set(gcf, 'Units', 'centimeters', 'Position', [0, 0, 9, 7]);
set(gca, 'FontName', 'Times New Roman', 'FontSize', 8)
print('Example_D_5_Figure_2.eps','-depsc')