108

Apndice C
Programa MATLAB Cinemtica Inversa
%Francisco Prez Marcial
%Tesis: Diseo, Anlisis y Modelado Cinemtico de un Exoesqueleto Pasivo
de
%Extremidad Inferior con Propsito de Rehabilitacin
%Este programa se encarga de calcular la cinemtca inversa de la caminata
%del exoesqueleto
% Cinematica_Inversa(px,py,pz)
% Funcin que recibe la posicin desada del tobillo con respeto a la
base
% del sistema.
% Por lo que la matriz que regresa es la configuracin final de los
% posibles angulos que puede asignarse al exoesqueleto.
function [Exo_thes] = Cinematica_Inversa(px, py, pz)

%Paramtros del Exoesqueleto LEGO
a2=14;
a3=13;
%primer angulo
%dos posibles soluciones por la ecuacin trascendental.
%py*c1+-px*s1=0 Ec. 52 Tesis
%Primera Solucin
theta1(1)= atan2( py, px);
if (theta1(1)< pi/3) &&(theta1(1)>-pi/6) %condicion de abduccion/Aduccin
else
theta1(1)=inf;
end
%disp('theta1_1');
%disp(theta1(1));
%Segunda Solucin
theta1(2)= atan2(-py,-px);
if (theta1(2)< pi/3) &&(theta1(2)>-pi/6) %condicion de abduccion/Aduccin
else
theta1(2)=inf;
end
%disp('theta1_2');
%disp(theta1(2));
%tercer angulo
%dos posibles soluciones
%c3=k the3= atan2(+-sqrt(1-k^2),k) Condicin Algebraica Ec55 Tesis
b=(px^2+py^2+pz^2-a2^2-a3^2)/(2*a3*a2);
%Primera Solucin
theta3(1) = atan2(sqrt(1-b^2),b);
if (theta3(1)<= pi/20) &&(theta3(1)>=-pi/4) %Restriccin angular
else
theta3(1)=inf;
end
%disp('theta3_1');
%disp(theta3(1));
%Segunda Solucin
theta3(2) = atan2(-sqrt(1-b^2),b);
if (theta3(2)<= pi/20) &&(theta3(2)>=-pi/4) %Restriccin angular
else
109

theta3(2)=inf;
end
%disp('theta3_2');
%disp(theta3(2));


%segundo angulo
%cuatro posibles soluciones
%a*c2+b*s2=c the2=atan2(b,a)+-atan2(sqrt(a^2+b^2-c^2),c) Ec. 60
%Se obtienen los terminos que ya fueron encontrados
phi_1= px*cos(theta1(1))+py*sin(theta1(1));
%disp('phi_1');
%disp(phi_1);
phi_2= px*cos(theta1(2))+py*sin(theta1(2));
%disp('phi_2');
%disp(phi_2);
c=a2+a3*b;
%Primera Solucin
theta2(1)=atan2(-pz,phi_1)+atan2(sqrt(phi_1^2+pz^2-c^2),c);
if (theta2(1)< pi/2) &&(theta2(1)>-pi/4) %Restriccin angular
else
theta2(1)=inf;
end
%Segunda Solucin
theta2(3)=atan2(-pz,phi_1)-atan2(sqrt(phi_1^2+pz^2-c^2),c);
if (theta2(3)< pi/2) &&(theta2(3)>-pi/4) %Restriccion angular
else
theta2(3)=inf;
end
%Tercera Solucin
theta2(2)=atan2(-pz,phi_2)+atan2(sqrt(phi_2^2+pz^2-c^2),c);
if (theta2(2)< pi/2) &&(theta2(2)>-pi/4) %Restricci angular
else
theta2(2)=inf;
end
%Cuarta Solucin
theta2(4)=atan2(-pz,phi_2)-atan2(sqrt(phi_2^2+pz^2-c^2),c);
if (theta2(4)< pi/2) &&(theta2(4)>-pi/4) %Restriccin angular
else
theta2(4)=inf;
end
%disp('theta2_1');
%disp(theta2(1));
%disp('theta2_2');
%disp(theta2(2));
%disp('theta2_3');
%disp(theta2(3));
%disp('theta2_4');
%disp(theta2(4));
%Se forma la matriz que tiene todos los ngulos
Exo_thes(1,:)= [theta1(1),theta1(2),0,0];
Exo_thes(2,:)= [theta2(1),theta2(2),theta2(2),theta2(2)];
Exo_thes(3,:)= [theta3(1),theta3(2),0,0];
end