%========================================================================
% arquivo: RetroEscavadeiraSimbolico.m
% Exemplo 3.3.1 do Doughty
% 
% iniciado em: 25/02/2015
% prof. Jorge Luiz Erthal
%========================================================================
clear all
close all
clc
syms C1 C2 C3 C4 C5 C6 C7 C8 C9 C10 C11 C12 C13 C14 C15 C16 C17
syms D1 D2
syms q1 q2 q3 A1 B1 A2 B2 A3 B3 A4 B4
syms dq1dt dq2dt dq3dt d2q1dt2 d2q2dt2 d2q3dt2
syms Ka11 Ka12 Ka13 Kb11 Kb12 Kb13 Ka21 Ka22 Ka23 Kb21 Kb22 Kb23
syms Ka31 Ka32 Ka33 Kb31 Kb32 Kb33 Ka41 Ka42 Ka43 Kb41 Kb42 Kb43
%------------------------------------------------------------------------
%% vetores
S=[A1; B1; A2; B2; A3; B3; A4; B4];
q=[q1; q2; q3];
dqdt=[dq1dt; dq2dt; dq3dt];
d2qdt2=[d2q1dt2; d2q2dt2; d2q3dt2];
%------------------------------------------------------------------------
%% equações cinemáticas de posição
f = [ D1-q1*cos(B1)-C1*sin(C16-A1-pi/2);
      D2-q1*sin(B1)+C1*cos(C16-A1-pi/2);
      C5*cos(A1)+C4*sin(A1)+C6*cos(A2)-C7*sin(A2)-q2*cos(A1+B2);
      C5*sin(A1)-C4*cos(A1)+C6*sin(A2)+C7*cos(A2)-q2*sin(A1+B2);
      C9*cos(A2)+q3*sin(A2+B3)-C12*cos(A3)-C10*sin(A2);
      C9*sin(A2)-q3*cos(A2+B3)+C12*sin(A3)+C10*cos(A2);
      C12*cos(A3)+C13*sin(A2-B4)-C14*cos(A4)-C11*sin(A2);
     -C12*sin(A3)-C13*cos(A2-B4)+C14*sin(A4)+C11*cos(A2)];
%------------------------------------------------------------------------
%% velocidades
J = jacobian(f,S);
Q = jacobian(f,q);
KK=-inv(J)*Q;
K=[Ka11 Ka12 Ka13; 
   Kb11 Kb12 Kb13;
   Ka21 Ka22 Ka23; 
   Kb21 Kb22 Kb23;
   Ka31 Ka32 Ka33; 
   Kb31 Kb32 Kb33;
   Ka41 Ka42 Ka43; 
   Kb41 Kb42 Kb43];
%%
for i=1:size(K,1)
    for j=1:size(K,2)
        if KK(i,j) == sym(0)
            K(i,j) = sym(0);
        end
    end
end

dSdt=K*dqdt;
dA1dt=dSdt(1);
dB1dt=dSdt(2);
dA2dt=dSdt(3);
dB2dt=dSdt(4);
dA3dt=dSdt(5);
dB3dt=dSdt(6);
dA4dt=dSdt(7);
dB4dt=dSdt(8);
%------------------------------------------------------------------------
%% acelerações
for i=1:size(S,1)
    for j=1:size(S,1)
        dJdq1(i,j)=jacobian(J(i,j),S)*K(:,1)+diff(J(i,j),q(1));
        dJdq2(i,j)=jacobian(J(i,j),S)*K(:,2)+diff(J(i,j),q(2));
        dJdq3(i,j)=jacobian(J(i,j),S)*K(:,3)+diff(J(i,j),q(3));
    end
end
for i=1:size(S,1)
    for j=1:size(q,1)
        dQdq1(i,j)=jacobian(Q(i,j),S)*K(:,1)+diff(Q(i,j),q(1));
        dQdq2(i,j)=jacobian(Q(i,j),S)*K(:,2)+diff(Q(i,j),q(2));
        dQdq3(i,j)=jacobian(Q(i,j),S)*K(:,3)+diff(Q(i,j),q(3));
    end
end
L1=-inv(J)*(dJdq1*K+dQdq1);
L2=-inv(J)*(dJdq2*K+dQdq2);
L3=-inv(J)*(dJdq3*K+dQdq3);
d2Sdt2=K*d2qdt2+L1*dqdt*dq1dt+L2*dqdt*dq2dt+L3*dqdt*dq3dt;
d2A1dt2=d2Sdt2(1);
d2B1dt2=d2Sdt2(2);
d2A2dt2=d2Sdt2(3);
d2B2dt2=d2Sdt2(4);
d2A3dt2=d2Sdt2(5);
d2B3dt2=d2Sdt2(6);
d2A4dt2=d2Sdt2(7);
d2B4dt2=d2Sdt2(8);
%------------------------------------------------------------------------
%% dados numéricos
C1=30;
C2=14;
C3=30;
C4=6;
C5=70;
C6=3;
C7=25;
C8=12;
C9=6;
C10=60;
C11=14;
C12=20;
C13=18;
C14=10;
C15=21;
C16=121.4671*pi/180;
C17=81.36*pi/180;
D1=25;
D2=15;
q1=46;
q2=76;
q3=70;
A1=0.219570;
A2=0.195236;
A3=0.270786;
A4=0.771711;
B1=1.231979;
B2=0.251588;
B3=0.170355;
B4=0.743913;
%------------------------------------------------------------------------
Kcalc=eval((-inv(J)*Q));
Ka11=Kcalc(1,1);
Ka12=Kcalc(1,2);
Ka13=Kcalc(1,3); 
Kb11=Kcalc(2,1);
Kb12=Kcalc(2,2);
Kb13=Kcalc(2,3);
Ka21=Kcalc(3,1);
Ka22=Kcalc(3,2);
Ka23=Kcalc(3,3); 
Kb21=Kcalc(4,1);
Kb22=Kcalc(4,2);
Kb23=Kcalc(4,3);
Ka31=Kcalc(5,1);
Ka32=Kcalc(5,2);
Ka33=Kcalc(5,3); 
Kb31=Kcalc(6,1);
Kb32=Kcalc(6,2);
Kb33=Kcalc(6,3);
Ka41=Kcalc(7,1);
Ka42=Kcalc(7,2);
Ka43=Kcalc(7,3); 
Kb41=Kcalc(8,1);
Kb42=Kcalc(8,2);
Kb43=Kcalc(8,3);

clc
for i=1:size(Kcalc,1)
    for j=1:size(Kcalc,2)
        fprintf('%10.6f',Kcalc(i,j))
    end
    fprintf('\n')
end
%%
%------------------------------------------------------------------------
Jcalc=eval(J);
dJdq1calc=eval(dJdq1);
dQdq1calc=eval(dQdq1);
L1calc=eval(L1);
clc
for i=1:size(L1calc,1)
    for j=1:size(L1calc,2)
        fprintf('%10.6f',L1calc(i,j)*100)
    end
    fprintf('\n')
end
%%
clc

for i=1:size(J,1)
    for j=1:size(J,2)
        if J(i,j) ~= sym(0)
            fprintf('J(%d,%d) = ',i,j)
            simplify(J(i,j))
        end
    end
end
%%
clc
for i=1:size(Q,1)
    for j=1:size(Q,2)
        if Q(i,j) ~= sym(0)
            fprintf('Q(%d,%d) = ',i,j)
            simplify(Q(i,j))
        end
    end
end
%%
clc
for i=1:size(dJdq1,1)
    for j=1:size(dJdq1,2)
        if dJdq1(i,j) ~= sym(0)
            fprintf('dJdq1(%d,%d) = ',i,j)
            simplify(dJdq1(i,j))
        end
    end
end
