function p=kindir2R(theta,a1,a2,plotta)

% Programmino per il calcolo della cinematica diretta 
% del robot planare 2R
% se la variabile stringa plotta='plot'
% allora calcola anche le coordinate del gomito
% e disegna il robot
% 
%    axis([-1 1 -1 1]*(a1+a2)*1.1), % axis equal
%    axis('square')

c1=cos(theta(1)); c12=cos(theta(1)+theta(2));
s1=sin(theta(1)); s12=sin(theta(1)+theta(2));

px=a1*c1+a2*c12;
py=a1*s1+a2*s12;

p=[px; py];

if strcmp(plotta,'plot')
    pgx=a1*c1; 
    pgy=a1*s1;
    PPX=[0 pgx px];
    PPY=[0 pgy py];
    plot(PPX,PPY,'o-','linewidth',4)
end



