An Analytical Algorithm for a Seven-DOF
Limb
“Real-time inverse kinematics techniques
for anthropomorphic limbs”
Deepak Tolani, Ambarish Goswami, Norman I. Badler
University of Pennsylvania
A 7 (or 4) degree-of-freedom limb
3 DOF
S1
3 DOF
L3
S2
1 DOF
L1
F
L2
Numerical approaches : Jacobian singularites, local
minima
Analytical approaches : efficient and reliable
Inverse Kinematics
Rotation matrices at joints S1, F, and S2
R ( , , ) 0
T1   1 1 2 3
1
 0 0 0
 R ( ) 0
Ty   y 4

0 0 0 1 
2 constant transformation matrices
ta 
 Ra
A

0 0 0 1 
tb 
 Rb
B

0 0 0 1 
A desired goal
tg 
 Rg
G

0
0
0
1


The equation to solve
T1 ATy BT2  G
R ( , , ) 0
T2   2 5 6 7
1
 0 0 0
Step 1: Solving for
4
Only  4 affects the distance of S2 relative to S1.
If the normal vector of the plane containing S1, S2, and F
is parallel to the axis of rotation of F, then  4 can be
computed trivially using the law of cosines.
L1  L2  L3
cos( 4 ) 
2 L1 L2
2
Else, …
2
3
Step 2: Solving for Elbow Position
nˆ 
tg
aˆ  (aˆ  nˆ )nˆ
uˆ 
aˆ  (aˆ  nˆ )nˆ
tg
L3  L1  L2
cos( ) 
2 L3 L1
2
2
vˆ  nˆ  uˆ
3
c  cos( ) L1nˆ
R  sin(  ) L1
e( )  c  R(cos( )uˆ  sin(  ) vˆ )
Step 3: Solving for R1 and R2
xˆ g ( ) yˆ g ( ) zˆ g ( ) 0 xˆ yˆ zˆ 0
T1  
 0 0 0 1
0
0
0
1



T2  (T1 ATy B)1 G
T
Class PV_IK
\\vrs4\opensources\xmath
\\vrs4\opensources\ik
#include “pv_ik.h”
…
float t4, r1[4][4];
…
PV_IK ik(L1, L2);
ik.setGoal(gx, gy, gz);
ik.solve();
//or ik.solve(psi);
t4 = ik.getTheta4();
ik.getR1(r1);