%========================================================================
% arquivo: QuatroBarras2glSimbolico2.m
% Exemplo 3.1.2 do Doughty
% 
% iniciado em: 25/02/2015
% prof. Jorge Luiz Erthal
%========================================================================
clear all
close all
clc
syms C1 C2 C3 X4 Y4
syms q0 q1 A2 A3 dq0dt dq1dt d2q0dt2 d2q1dt2
syms Ka20 Ka21 Ka30 Ka31
%------------------------------------------------------------------------
%% vetores
S=[A2; A3];
q=[q0; q1];
dqdt=[dq0dt; dq1dt];
d2qdt2=[d2q0dt2; d2q1dt2];
%------------------------------------------------------------------------
%% equações cinemáticas de posição
f = [q0+C1*cos(q1)+C2*cos(A2)+C3*cos(A3)-X4;
        C1*sin(q1)+C2*sin(A2)+C3*sin(A3)-Y4];
%------------------------------------------------------------------------
%% velocidades
J = jacobian(f,S)
Q = jacobian(f,q)
% K=[Ka20 Ka21;
%    Ka30 Ka31];
K=-inv(J)*Q
Ka20=K(1,1);
Ka21=K(1,2);
Ka30=K(2,1);
Ka31=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)
        dJdq0(i,j)=jacobian(J(i,j),S)*K(:,1)+diff(J(i,j),q(1));
        dJdq1(i,j)=jacobian(J(i,j),S)*K(:,2)+diff(J(i,j),q(2));
    end
end
dJdq0=simplify(dJdq0)
dJdq1=simplify(dJdq1)

for i=1:size(S,1)
    for j=1:size(q,1)
        dQdq0(i,j)=jacobian(Q(i,j),S)*K(:,1)+diff(Q(i,j),q(1));
        dQdq1(i,j)=jacobian(Q(i,j),S)*K(:,2)+diff(Q(i,j),q(2));
    end
end
dQdq0=simplify(dQdq0)
dQdq1=simplify(dQdq1)

L0=simplify(-inv(J)*(dJdq0*K+dQdq0));
L1=simplify(-inv(J)*(dJdq1*K+dQdq1));

d2Sdt2=K*d2qdt2+L0*dqdt*dq0dt+L1*dqdt*dq1dt;
d2A2dt2=d2Sdt2(1);
d2A3dt2=d2Sdt2(2);


