hola a todos.
estoy haciendo un robot de 5 gdl (todos rotaciones) y no consigo que me funcione la cinematica inversa... Si alguien que sepa me puede echar una mano seria genial. El codigo que estoy usando es este (en Matlab, que lo estoy probando ahi para luego usarlo con el ardino)
function q=CIN_INVERSA(x, y, z, gamma)
% Inicialización de las variables articulares a calcular
q1=0;
q2=0;
q3=0;
q4=0;
q5=0;
% %Valores de las longitudes
% d1=90;
% d2=60;
% d3=58.22;
% d4=51.5; %Incluye el masajeador
% d5=260;
%Valores de las longitudes
d1=85;
d2=60;
d3=58.22;
d4=311.5;
%Primer angulo
q1=atan2(y,x)
%Tercer ángulo (positivo)
r=sqrt(x^2+y^2);
gamma_rad=gammapi/180;
r_p=r-d4sin(gamma_rad);
z_p=(z-d1)-d4*cos(gamma_rad);
%h^2=r_p^2+z_p^2= d2^2+d3^2+2d2d3*cos(180-q3) (Th.coseno)
A=(r_p^2+(z_p)^2-d2^2-d3^2)/(2d2d3) %A=cosq3
m=sqrt((1-A^2))
q3=atan2(m,A)+pi/2
%Segundo ángulo
beta=atan2((z_p-d1),r_p);
alpha1=d3sin(q3);
alpha2=d2+d3cos(q3);
alpha=atan2(alpha1,alpha2);
q2=beta-alpha+pi/2;
%Resultado en grados
% q1=q1180/sym('pi');
% q2=q2180/sym('pi');
% q3=q3180/sym('pi');
% q4=q4180/sym('pi');
% q5=q5*180/sym('pi');
%Cuarto ángulo
q4=q2-q3-gamma_rad;
% q1=q1;
% q2=q2;
% q3=q3;
% q4=q4+90;
% q5=q5;
q=vpa([q1 q2 q3 q4 q5],5);
end
el quinto angulo no afecta a la posicion y el gamm es un angulo que se introduce como dato.
Gracias
