%========================================================================
% arquivo: QuatroBarras1glSimbolico.m
% Exemplo 2.2 do Doughty
% 
% iniciado em: 25/02/2015
% prof. Jorge Luiz Erthal
%========================================================================
clear all
close all
clc
syms C1 C2 C3 C4
syms q1 A2 A3 dq1dt d2q1dt2
syms Ka2 Ka3
syms Up Vp
%------------------------------------------------------------------------
%% vetores
S=[A2; A3];
q=[q1];
dqdt=[dq1dt];
d2qdt2=[d2q1dt2];
%------------------------------------------------------------------------
%% equações cinemáticas de posição
f = [C1*cos(q1)+C2*cos(A2)+C3*cos(A3)-C4;
     C1*sin(q1)+C2*sin(A2)+C3*sin(A3)];
%------------------------------------------------------------------------
%% referência dos pontos de interesse
Ori = [C1*cos(q1);
       C1*sin(q1)];
Rot = [cos(A2) -sin(A2);
       sin(A2)  cos(A2)];
Ploc = [Up; Vp];
%------------------------------------------------------------------------
%% velocidades
J = jacobian(f,S);
Q = jacobian(f,q);
% K=[Ka1 Ka2; 
%    Kb1 Kb2];
K=simplify(-inv(J)*Q);
Ka2=K(1,1);
Ka3=K(2,1);
dSdt=K*dqdt;
dA2dt=dSdt(1);
dA3dt=dSdt(2);
%------------------------------------------------------------------------
%% acelerações
for i=1:size(S,1)
    for j=1:size(S,1)
        dJdq1(i,j)=simplify(jacobian(J(i,j),S)*K(:,1)+diff(J(i,j),q(1)));
    end
end
for i=1:size(S,1)
    for j=1:size(q,1)
        dQdq1(i,j)=simplify(jacobian(Q(i,j),S)*K(:,1)+diff(Q(i,j),q(1)));
    end
end
Jinv=simplify(inv(J));
L1=simplify(-Jinv*(dJdq1*K+dQdq1));
d2Sdt2=K*d2qdt2+L1*dqdt*dq1dt;
d2A2dt2=d2Sdt2(1);
d2A3dt2=d2Sdt2(2);
%------------------------------------------------------------------------
%% pontos de interesse
%% posição
Pglob = simplify(Ori + Rot*Ploc);
Xp = Pglob(1);
Yp = Pglob(2);
%% coeficientes de velocidade
for i=1:size(S,1)
    for j=1:size(q,1)
        dOridq1(i,j)=simplify(jacobian(Ori(i,j),S)*K(:,1)+diff(Ori(i,j),q(1)));
    end
end
for i=1:size(S,1)
    for j=1:size(S,1)
        dRotdq1(i,j)=simplify(jacobian(Rot(i,j),S)*K(:,1)+diff(Rot(i,j),q(1)));
    end
end
Kp = simplify(dOridq1 + dRotdq1*Ploc);
Kpx = Kp(1);
Kpy = Kp(2);

return   % interrupção do programa simbólico
%------------------------------------------------------------------------
%% dados numéricos
C1=5;
C2=9;
C3=7;
C4=10;
q1=1.0472;
A2=16.653772*pi/180;
A3=99.227513*180/pi;
dq1dt=1;
d2q1dt2=1;
Up=C2;
Vp=0;
%------------------------------------------------------------------------
dA2dtcalc=eval(dA2dt);
dA3dtcalc=eval(dA3dt);

d2A2dt2calc=eval(d2A2dt2);
d2A3dt2calc=eval(d2A3dt2);

Xpcalc = eval(Xp);
Ypcalc = eval(Yp);
%------------------------------------------------------------------------
fprintf('A2 = %f\n',A2*180/pi)
fprintf('A3 = %f\n',A3*180/pi)
fprintf('dA2dt = %f\n',dA2dtcalc)
fprintf('dA3dt = %f\n',dA3dtcalc)
fprintf('d2A2dt2 = %f\n',d2A2dt2calc)
fprintf('d2A3dt2 = %f\n',d2A3dt2calc)

fprintf('Xp = %f\n',Xpcalc)
fprintf('Yp = %f\n',Ypcalc)