function theta=kininv3R(p,phi,a1,a2,a3)

% calcola le due soluzioni gomito UP e gomito DOWN
% della cinematica inversa del robot planare 3R
% Descrizione input
% p= [px; py] : coordinate cartesiane
% phi : angolo di orientamento mano
% a1, a2 e a3 : lunghezze dei bracci
% Descrizione Output
% theta(:,1) soluzione DOWN:  theta(2,1)>0
% theta(:,2) soluzione UP:    theta(2,2)=-theta(2,1)

theta=zeros(3,2);

px=p(1); py=p(2);
cphi=cos(phi); sphi=sin(phi);

% Calcolo della posizione del polso
pwx=px-a3*cphi;
pwy=py-a3*sphi;

th=kininv2R(pwx,pwy,a1,a2);

theta(1:2,1:2)=th;
theta(3,1)=phi-theta(1,1)-theta(2,1);
theta(3,2)=phi-theta(1,2)-theta(2,2);


        
    

