Inverse Kinematics Tim Haines

filercaliforniaΜηχανική

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

81 εμφανίσεις

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