% Cálculo da cinemática directa

function xy=cinematdir(theta,L)

% Comprimento dos elos
L1=L(1);
L2=L(2);

theta1=theta(1);
theta2=theta(2);

% Cálculo da cinemática directa
x1=L1*cos(theta1);
y1=L1*sin(theta1);
x2=x1+L2*cos(theta1+theta2);
y2=y1+L2*sin(theta1+theta2);

% Retorno do resultado
xy=[x1 y1; x2 y2];