function [p,phi]=kindir3R(theta,a1,a2,a3,plotta)

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

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

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

p=[px; py]; phi=

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



