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 00

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.30

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.60

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.70

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2