# Inverse Kinematics Tim Haines

Μηχανική

14 Νοε 2013 (πριν από 4 χρόνια και 5 μήνες)

104 εμφανίσεις

Inverse Kinematics

Tim Haines

Geometry based solution

Circle Intersection

Number of Arms = n

Solution finds the intersection of 2*n
-
1
circles

For each goal node there are between 4
and 0 solutions (using three links)

clear all; clc;

A = [0 0]; %# Origin

B = [0 2.0]; %# center of goal node

l1 = 1.00; %# Length of base member

l2=1.00; %Length of second member

l3=0.5; %length of third member

r1 = 1.25; %# radius of the SECOND circle

%%Determines the location of first arm%%

c = norm(A
-
B) %# distance between circles

cosAlpha = (l1^2+c^2
-
r1^2)/(2*l1*c);

u_AB = (B
-

A)/c; %# unit vector from first to second center

pu_AB = [u_AB(2),
-
u_AB(1)]; %# perpendicular vector to unit vector

%# use the cosine of alpha to calculate the length of the

%# vector along and perpendicular to AB that leads to the

%# intersection point

inter1 = A + u_AB * (l1*cosAlpha) + pu_AB * (l1*sqrt(1
-
cosAlpha^2))

inter2 = A + u_AB * (l1*cosAlpha)
-

pu_AB * (l1*sqrt(1
-
cosAlpha^2))

%%Determines the location of the second and third arm Using inter1 %%

D=inter1;

d = norm(D
-
B) %# distance between circles

cosAlpha1 = (l2^2+d^2
-
l3^2)/(2*l2*d);

u_AB1 = (B
-

D)/d; %# unit vector from first to second center

pu_AB1 = [u_AB1(2),
-
u_AB1(1)]; %# perpendicular vector to unit vector

%# use the cosine of alpha to calculate the length of the

%# vector along and perpendicular to AB that leads to the

%# intersection point

inter3 = D + u_AB1 * (l1*cosAlpha1) + pu_AB1 * (l1*sqrt(1
-
cosAlpha1^2))

inter4 = D + u_AB1 * (l1*cosAlpha1)
-

pu_AB1 * (l1*sqrt(1
-
cosAlpha1^2))

%Plot Solution 1

figure(1)

line([A(1) inter1(1)] ,[A(2) inter1(2)],[0 0],'Marker','.','LineStyle','
-
')

line([inter1(1) inter3(1)],[inter1(2) inter3(2)],[0 0],'Marker','.','LineStyle','
-
')

line([inter3(1) B(1)],[inter3(2) B(2)],[0 0],'Marker','.','LineStyle','
-
')

-0.7
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0.4
0.5
0.6
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2