% Manipolabilita di un robott a cavi come da figura
% Il robot lavora nel piano verticale. I due cavi, avvolti attorno ai rocchetti, tirano i cavi che scorrono sui passacavi A e B.
% L'obiettivo consiste nel tracciare l'elissoide di manipolabilità (di velocità e forza) riferito all'end-effector nel punto P.
% Dati geometrici:

clear all
clf
h=5             % altezza dello spazio di lavoro
l=6             % larghezza
r=1             % raggio dei tamburi di avvolgimento

% Coordiante del punto di dove si vuole indagare la manipolabilità:

Pm=[l/2,h-1]'

% Nota: si tratta di un robot parallelo.
%
% Prima di tutto ci si occupa della cinamatica inversa (la diretta per un robot parallelo costituisce un problema non lineare)
% Se si assume che l'end-effector stia sul punto A quando q1=q2=0, si ottiene:
% l1=r*q1
% l2=l+r*q2
% Sviluppando l'analisi cinematica inversa si ottiene:
%
% Perciò, dato il punto Pm, i valori ai giunti sono:

x=Pm(1);
y=Pm(2);
q1=(1/r)*sqrt(x^2+(y-h)^2)
q2=(1/r)*(sqrt((x-l)^2+(y-h)^2)-l)

% Grafico della posizione dell'end-effector

figure(1)
hold on
axis equal
plot([0,l,l,0,0],[0,0,h,h,0],'g')   % spazio di lavoro
plot([0,x],[h,y],'r')               % cavo l1
plot([l,x],[h,y],'r')               % cavo l1

% Partendo dalle equazioni della cinematica elevate al quadrato:
%
% si deriva rispetto al tempo:
%
% E ponendo in forma matriciale, estraendo i termini che derivano dal tempo, si ottiene:
%

MA=[x      y-h ;...
    x-l    y-h ]

MB=[r^2*q1  0             ;...
    0       r*(l+r*q2) ]

% Per ciò lo Jacobiano è pari a:

J=inv(MA)*MB

% Secondo la teoria degli ellissoidi di manipolabilità, per quanto riguarda le velocità, la matrice di cui calcolare gli autovalori ed autovettori è:

M=J*J'

% Calcolo degli autovettori ed autovalori:

[Autovettori,Autovalori]=eig(M)

% Notare che si si sono calcolati gli autovettori ed autovalori di (J*J'), dato che si sta individuando l'ellissoide di manipolabilità delle velocità.
% Rappresentazione grafica degli assi dell'ellisse di manipolabilità di velocità:

figure(2)    
hold on
axis equal
c1=sqrt(Autovalori(1,1));       % radice degli autovalori
c2=sqrt(Autovalori(2,2));
plot(c1*[0,Autovettori(1,1)],c1*[0,Autovettori(2,1)],'r')
plot(c2*[0,Autovettori(1,2)],c2*[0,Autovettori(2,2)],'r')

% Si traccia l'ellissoide di manipolabilità per quanto riguarda le forze:

figure(3)    
hold on
axis equal
[Autovettori,Autovalori]=eig(inv(M)) % autovalori dell'inversa di J*J'
c1=sqrt(Autovalori(1,1));       % radice degli autovalori
c2=sqrt(Autovalori(2,2));
plot(c1*[0,Autovettori(1,1)],c1*[0,Autovettori(2,1)],'b')
plot(c2*[0,Autovettori(1,2)],c2*[0,Autovettori(2,2)],'b')

%