INVKIN

Table of Contents
Description ......................................................................................................................... 1
Calling Syntax .................................................................................................................... 1
I/O Variables ...................................................................................................................... 1
Function ............................................................................................................................ 1
Main Calculations ............................................................................................................... 1
Example ............................................................................................................................ 3

Description
Calcula a cinemtica inversa

Calling Syntax
function [near,far,sol] = invkin(wrelb,current,L,thetalim)

I/O Variables
IN: wrelb a matriz de transformao do punho para a base e descreve para onde deseja-se levar o rob;

IN: current a posio atual do rob (dada como um vetor de ngulos das juntas) e serve como infor-
mao para de?nir qual a soluo mais prxima;

IN: L um vetor de comprimentos dos ligamentos;

IN: thetalim uma matriz 2 x n de limites superiores e inferiores de operao para cada varivel de junta;

OUT: near a soluo mais prxima;

OUT: far a segunda soluo;

OUT: sol um flag que indica se solues foram encontradas. Assim, sol=0 se nenhuma soluo foi
encontrada.

Function
function [near,far,sol] = invkin(wrelb,current,L,thetalim)

Main Calculations
u_f=itou(wrelb);
x=u_f(1);
y=u_f(2);
phi=u_f(3);

1
 INVKIN

c2 = (x^2+y^2-L(1)^2-L(2)^2)/(2*L(1)*L(2));
s2_a = sqrt(1-c2^2);
s2_b = -s2_a;

theta2_a=atan2d(s2_a,c2);
k1_a=L(1)+L(2)*c2;
k2_a=L(2)*s2_a;

gama_a=atan2d(k2_a,k1_a);
theta1_a=atan2d(y,x)-gama_a;
theta3_a=phi-theta1_a-theta2_a;

theta2_b=atan2d(s2_b,c2);
k1_b=L(1)+L(2)*c2;
k2_b=L(2)*s2_b;

gama_b=atan2d(k2_b,k1_b);
theta1_b=atan2d(y,x)-gama_b;
theta3_b=phi-theta1_b-theta2_b;

theta_a=[theta1_a theta2_a theta3_a];

theta_b=[theta1_b theta2_b theta3_b];

for i = 1:3
while (theta_a(i) > 180)
theta_a(i) = theta_a(i)-180;
end
while (theta_b(i) > 180)
theta_b(i) = theta_b(i)-180;
end
while (theta_a(i) < -180)
theta_a(i) = theta_a(i)+180;
end
while (theta_b(i) < -180)
theta_b(i) = theta_b(i)+180;
end
end

d_a = norm(current-theta_a);
d_b = norm(current-theta_b);

far = theta_b;
near = theta_a;

if d_a >= d_b

near = theta_b;
far = theta_a;
end

sol_a = 1;
sol_b = 1;

2
 INVKIN

sol = 1;

for i = 1:3
if ((theta_a(i) > thetalim(1,i)) || (theta_a(i) < thetalim(2,i)))
sol_a = 0;
end
if ((theta_b(i) > thetalim(1,i)) || (theta_b(i) < thetalim(2,i)))
sol_b = 0;
end
end

if (sol_a == 0) && (sol_b == 0)

sol=0;
end

Example
wrelb = kin([20,30,40],[1,1,1]);

current = [0,0,0];

L = [1,1,1];

thetalim = [170 170 170;-170 -170 -170];

[near,far,sol] = invkin(wrelb,current,L,thetalim)

end

near =

20.0000 30.0000 40.0000

far =

50.0000 -30.0000 70.0000

sol =

Published with MATLAB R2016a