Transcript IK.ppt
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
0
0
0
1
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);