clc close all clear all

%numero di link prompt = ('numero di link'); dlg_title = strcat('Inserire n'); num_lines = 1; def = {'3',}; answer = inputdlg(prompt,dlg_title,num_lines,def); n=str2num(cell2mat(answer(1,1))); eval(sprintf('T%de =[0,0,1,0;0,1,0,0;-1,0,0,0;0,0,0,1];',n)); eval(sprintf('R%de =[0,0,1;0,1,0;-1,0,0;];',n)); Z0=[0,0,1]'; gamma=0.00001; time(1,1)=0; dt=0.0001; %goal prompt = {'x','y','z'}; dlg_title = strcat('Inserire x,y,z'); num_lines = 1; def = {'15','0','0'}; answer = inputdlg(prompt,dlg_title,num_lines,def); Pg(1,1)=str2num(cell2mat(answer(1,1))); Pg(2,1)=str2num(cell2mat(answer(2,1))); Pg(3,1)=str2num(cell2mat(answer(3,1)));

prompt = {'Inserire l angolo q goal',}; dlg_title = strcat('Inserire l angolo q goal'); num_lines = 1; def = {'0'}; answer = inputdlg(prompt,dlg_title,num_lines,def); qg(1,1)=str2num(cell2mat(answer(1,1)));

R0g = [cos(qg),-sin(qg),0;sin(qg),cos(qg),0;0,0,1]; T0g = R0g; T0g(:,4)=[Pg(1,1);Pg(2,1);Pg(3,1)]; T0g(4,:)=[0,0,0,1];

eval(sprintf('T%de =[0,0,1,0;0,1,0,0;-1,0,0,0;0,0,0,1];',n)); Z0=[0,0,1]'; for i=1:n prompt = {strcat('Inserire q',num2str(i)),strcat('Inserire l',num2str(i))}; dlg_title = strcat('Inserire q',num2str(i)); num_lines = 1; def = {'0','10'}; answer = inputdlg(prompt,dlg_title,num_lines,def); q=str2num(cell2mat(answer(1,1))); l=str2num(cell2mat(answer(2,1))); eval(sprintf('q%d = [q];', i)); eval(sprintf('l%d = [l];', i)); end

for t=1:100 for i=1:n P0=[0,0,0,1]';

eval(sprintf('R%d%d = [cos(q%d(t,1)),-

sin(q%d(t,1)),0;sin(q%d(t,1)),cos(q%d(t,1)),0;0,0,1];',i-1,i,i,i,i,i)); eval(sprintf('T%d%d = R%d%d;',i-1, i,i-1, i)); eval(sprintf('T%d%d(:,4) = [l*cos(q%d(t,1));l*sin(q%d(t,1));0];',i-1, i,i,i)); eval(sprintf('T%d%d(4,:) = [0,0,0,1];',i-1, i)); if i==1 else eval(sprintf('T0%d = T0%d*T%d%d;',i,i-1,i-1,i)); eval(sprintf('R0%d = R0%d*R%d%d;',i,i-1,i-1,i)); end if i==n eval(sprintf('Z%d = R0%d*Z0;',i,i)); eval(sprintf('Pe = T0%d*P0;',i)); eval(sprintf('Pe(4,:)=[];')); else eval(sprintf('Z%d = R0%d*Z0;',i,i)); eval(sprintf('P%d = T0%d*P0;',i,i)); eval(sprintf('P%d (4,:)=[];',i)); end P0 (4,:)=[];

end for i=1:n eval(sprintf('JP%d=cross(Z%d,(Pe-P%d));',i,i-1,i-1)); eval(sprintf('JO%d=Z%d;',i,i-1)); eval(sprintf('Jcompleta(:,i)=[JO%d;JP%d];',i,i)); end J=Jcompleta; J(6,:)=[]; J(2,:)=[]; J(1,:)=[];

eval(sprintf('R0e=R0%d*R%de;',n,n)); %vector units lemma i0e=R0e(:,1); j0e=R0e(:,2); k0e=R0e(:,3); i0g=R0g(:,1); j0g=R0g(:,2); k0g=R0g(:,3);

% lemma1=cross(i0e,i0g)+cross(j0e,j0g)+cross(k0e,k0g); % lemma2=dot(i0e,i0g)+dot(j0e,j0g)+dot(k0e,k0g); % % senalpha=lemma1(3,1)/2; % cosalpha=(1-lemma2)/2; % alpha=atan2(senalpha,cosalpha);

alpha=-qg; for i=1:n eval(sprintf('alpha=alpha+q%d(t,1);',i)); end error_completo=[0;0;alpha;(Pg-Pe)]; error=error_completo; error(6,:)=[]; error(2,:)=[];

error(1,:)=[]; error_save(:,t)=error;

I=eye(3); xdot=error'*gamma*I; Jpinv=pinv(J); qdot=xdot*Jpinv';

for i=1:n eval(sprintf('q%d(t+1,1)=q%d(t,1)+qdot(1,i)*dt;',i,i)); end time(t+1,1)=time(t,1)+dt; Pe_save(:,t)=Pe; end

% q=zeros(n,1); % q=[0;0.2;0.2]; % l=[1;2;1]; % % %definisco le matrici di rotazione % % R01=[cos(q(1,1)),-sin(q(1,1)),0;sin(q(1,1)),cos(q(1,1)),0;0,0,1]; % R12=[cos(q(2,1)),-sin(q(2,1)),0;sin(q(2,1)),cos(q(2,1)),0;0,0,1]; % R23=[cos(q(3,1)),-sin(q(3,1)),0;sin(q(3,1)),cos(q(3,1)),0;0,0,1]; % % %definisco la matrice T di trasformazione % % T01=R01; % T01(:,4)=[l(1,1)*cos(q(1,1));l(1,1)*sin(q(1,1));0]; % T01(4,:)=[0,0,0,1]; % T12=R12; % T12(:,4)=[l(2,1)*cos(q(2,1));l(2,1)*sin(q(2,1));0]; % T12(4,:)=[0,0,0,1]; % T23=R23; % T23(:,4)=[l(3,1)*cos(q(3,1));l(3,1)*sin(q(3,1));0]; % T23(4,:)=[0,0,0,1]; % % %matrice endeffector "cambio di assi" % % T3e=[0,0,1,0;0,1,0,0;-1,0,0,0;0,0,0,1]; % % T0e=T01*T12*T23*T3e; % T02=T01*T12; % T03=T02*T23; % % %definisco le J % % Z0=[0,0,1]'; % Z1=R01*Z0;

% Z2=R01*R12*Z0; % Z3=R01*R12*R23*Z0; % % %posizione della matrice dell'endeffector % % P0=[0,0,0,1]'; % Pe=T03*P0; % P1=T01*P0; % P2=T02*P0; % P0(4,:)=[]; % Pe(4,:)=[]; % P1(4,:)=[]; % P2(4,:)=[]; % % %matrici Jacobiane Lineari % % JP1=cross(Z0,(Pe-P0)); % JP2=cross(Z1,(Pe-P1)); % JP3=cross(Z2,(Pe-P2)); % % %matrici Jacobiane Angolari % % JO1=Z0; % JO2=Z1; % JO3=Z2; % % %matrice Jacobiana % % Jcompleta(:,1)=[JO1;JP1]; % Jcompleta(:,2)=[JO2;JP2]; % Jcompleta(:,3)=[JO3;JP3]; % % %Jacobiana nel piano % J=Jcompleta; % % J(6,:)=[]; % J(2,:)=[]; % J(1,:)=[]; %