Inverse Kinematics –IKS Solutions
ME 4135 – Robotics and Controls
R.R. Lindeke, Ph.D., Fall 2011
FKS vs. IKS
• In FKS we built a tool for finding end frame
geometry from Given Joint data:
Joint
Space
Cartesian
Space
• In IKS we need Joint models from given End
Point POSE Geometry:
Cartesian
Space
Joint
Space
So this IKS is ‘Nasty’ – it is a so-call Hard
Mathematical Problem
• It a more difficult problem because:
– The Equation set is “Over-Specified”
• 12 equations in 6 unknowns
– Space may be “Under-Specified”
• for example ‘Planer devices’ with more joints than 2
– The Solution set can contain Redundancies
• Thus they exhibit Multiple solutions
– The Solution Sets may be un-defined
• Unreachable in 1 or many joints
But the IKS in Critical in Robotics
• Builds Workspace limits and controller maps
• Allows “Off-Line Programming” solutions
• Thus, compares Workspace capabilities with
Programming desirability to assure that execution is
feasible
• Aids in Workplace design and operational
simulations
Doing a Pure IKS solution: the R
Manipulator
X2
X1
Y0
Z1
Y1
Z2
Y2
X0
Z0
R Frame Skeleton
LP Table and Ai’s
Frames
Link
Var

d
l

S
C
S
C
0→1
1
R
 + 90
0
0
90
1
0
C1
-S1
1→2
2
P
0
d2 + cl2
0
0
1
0
1
0
  S1
 C1
A1  
 0

 0
0 C1 0 
0 S1 0 

1 0 0

0 0 1
1
0
A2  
0

0
0 0
0 
1 0
0 

0 1 d 2  cl2 

0 0
1 
FKS is A1*A2:
  S1
 C1

 0

 0
0 C1 0  1 0


0 S1 0
0 1

1 0 0 0 0
 
0 0 1  0 0
  S1
 C1

 0

 0
0
0 

0
0

1 d 2  cl2 

0
1 
0 C1 C1 (d 2  cl2 ) 
0 S1 S1 (d 2  cl2 ) 


1 0
0

0 0
1

Forming The IKS:
  S1
 C1

 0

 0
0 C1 C1 ( d 2  cl2 ) 
0 S1 S1 (d 2  cl2 ) 


1 0
0

0 0
1

 nx
n
 y
 nz

0
sx
sy
sz
ax
ay
az
0
0
dx 
dy 

dz 

1
Forming The IKS:
• Examining the two sides of this FKS equation
– n, s, a and d are given in an inverse sense – since we desire to
put the manipulator on a particular target
– But we will build the general solution only
• Term (1, 4) & (2,4) both side allow us to find an equation
for :
• Select (1,4): C1*(d2+cl2) = dx
• Select (2,4): S1*(d2+cl2) = dy
• Form a ratio of Term2,4 by Term1,4 to build an
equation for Tan() and then :
• S1/C1 = dy/ dx
• Tan  = dy/dx
•  = Atan2(dx, dy)
Forming The IKS:
• After  is found, back substitute and
solve for d2:
• Choose term(1,4): C1*(d2+cl2) = dx
• Isolating d2: d2 = [dx/C1] - cl2
Alternative Method – doing a pure inverse
approach
• Form [A1]-1 then pre-multiply both side by this
‘inverse’
• Leads to: A2 = A1-1*T0ngiven
1
0

0

0
0 0
0    S1 C1
1 0
0   0
0

0 1 d 2  cl2   C1 S1
 
0 0
1   0
0
0 0   nx
1 0  ny

0 0   nz
 
0 1  0
sx
sy
sz
ax
ay
az
0
0
dx 
dy 

dz 

1
After Simplifying the RHS:
1
0

0

0
  S1 nx  C1 n y 

nz


  C1 nx  S1 n y 

0

  S1 s
C1 s
0 0
0 
1 0
0 

0 1 d 2  cl2 

0 0
1 
x
 C1 s y 
sz
x
 S1 s y 
0
  S1 a
C1 a
x
 C1 a y 
az
x
 S1 a y 
0
  S1 d
 C1 d y  

dz


C1 d x  S1 d y  

1

x
Solving:
• Selecting and Equating terms (1,4)
•
•
•
•
0 = -S1*dx + C1*dy
Solving: S1*dx = C1*dy
Tan() = (S1/C1) = (dy/dx)
 = Atan2(dx, dy)
• Selecting and Equating terms (3,4) -- after back
substituting  solution – and note, after the
above step,  is known as an angle
• d2 + cl2 = C1*dx + S1*dy
• d2 = C1*dx + S1*dy - cl2
d2 = Cos[Atan2(dx, dy)]*dx + Sin[Atan2(dx, dy)]*dy –cl2
Performing IKS For Industrial Robots:
• First lets consider the previously defined
Spherical Wrist simplification
– All Wrist joint Z’s intersect at a point
– The n Frame is offset from this intersection as a
distance dn along the a vector of the desired
solution (3rd column of desired orientation submatrix)
– This is as expected by the DH Algorithm
Performing IKS
• We can now separate the effects of the ARM joints
– Joints 1 to 3 in a full function manipulator (without
redundant joints)
– They function to position the spherical wrist at a target
POSITION related to the desired target POSE
• Arm Joints are separated from the WRIST Joints
– Joints 4 to 6 in a full functioning spherical wrist
– Wrist Joints function as a primary tool to ORIENT the end
frame as required by the desired target POSE
Performing IKS: Focus on Positioning
• We will define a point (the WRIST CENTER) as:
– Pc = [Px, Py, Pz]
– Here we define Pc = dtarget - dn*a
• Px = dtarget,x - dn*ax
• Py = dtarget,y - dn*ay
• Pz = dtarget,z - dn*az
Focusing on the ARM Manipulators in terms
of Pc:
• Prismatic:
• q1 = Pz (its along Z0!) – cl1
• q2 = Px or Py - cl2
• q3 = Py or Px - cl3
• Cylindrical:
• 1 = Atan2(Px, Py)
• d2 = Pz – cl2
• d3 = Px*C1 – cl3 or +(Px2 + Py2).5 – cl3
Focusing on the ARM Manipulators in terms
of Pc:
• Spherical:
• 1 = Atan2(Px, Py)
• 2 = Atan2( (Px2 + Py2).5 , Pz)
• D3 = (Px2 + Py2 + Pz2).5 – cl3
Focusing on the ARM Manipulators in terms
of Pc:
• Articulating:
• 1 = Atan2(Px, Py)
• 3 = Atan2(D, (1 – D2).5)
2
2
2
2
2
P

P

P

l

l
 x y z 2 3
– Where D =
2l2l3
• 2 =  - 
–  is: Atan2((Px2 + Py2).5, Pz)
–  is:
A tan 2(cos  ,sin  ) 


A tan 2  Px2  Py2  Pz2  l22  l32  , 2l2l3  1  D 2 

One Further Complication:
• This is called the d2 offset problem
• A d2 offset is a problem that states that the n
frame has a non-zero offset along the Y0 axis
as observed with all joints at home, in the
solution of the 0Tn
• This leads to two solutions for 1 the So-Called
Shoulder Left and Shoulder Right solutions
Defining the d2 Offset issue
The ARM
Z0
d
The
Wrist
Zpc
Xpc
2
Y0, Z1
Ypc
X0, X1
Here: ‘The ARM’ might be a prismatic joint as in the Stanford Arm or it might be
l2 & l3 links in an Articulating Arm and rotates out of plane
A d2 offset means that there are two places where 1 can be placed to touch a
given point (and note, when 1 is at Home, the wrist center is not on the X0
axis!)
Lets look at this Device “From the Top”
Pc'(Px, Py)
Y0
a
L33’'
Z1
d2
L2a’ 2'
R'
d2
X1
1
X

11
X0
Z1'
X1'
Solving For 1
• We will have a Choice (of two) poses for :
 11  1  1
 11  A tan 2( X pc , Ypc )

 A tan 2  X
2
pc
Y  d
2
pc

2 .5
2
 12  180  1  1 

180  A tan 2  X
2
pc
Y  d
A tan 2  X pc , Ypc 
2
pc

2 .5
2
, d2


, d2 
In this so-called “Hard Arm”
• We have two 1’s
• These lead to two 2’s (Spherical)
• Or to four 2’s and 3’s in the Articulating Arm
• Shoulder Right Elbow Up & Down
• Shoulder Left Elbow Up & Down
The Orientation Model
• Evolves from:
R R  Rgiven
3
0
• Separates Arm Joint and
Wrist Joint Contribution
to Target (given)
orientation
6
3
Focusing on Orientation Issues
• Lets begin by
considering Euler
Angles (they are a
model that is almost
identical to a full
functioning Spherical
Wrist):
• Form Product:
– Rz1*Ry2*Rz3
– This becomes R36
cos   sin  0 
Rz1   sin  cos  0 


 0
0
1 
 cos 0 sin  
Ry 2   0
1
0 


  sin  0 cos 
cos  sin 0 
Rz 3   sin cos 0 


 0
0
1 
Euler Wrist Simplified:
C C C  S S
 S C C  C S


 S C
C C S  S C
 S C S  C C
S S
C S 
S S 

C 
And this matrix is equal to a U matrix prepared by multiplying the inverse of the ARM
joint orientation matrices inverse and the Desired (given) target orientation
R 
3 1
0
NOTE: R03 is
Manipulator
dependent!
 nx
n
 y
 nz
sx
sy
sz
ax 

ay

az  given
Simplifying the RHS: (our so-called U Matrix)
 R11 R12 R13 
R03   R 21 R 22 R 23


 R31 R32 R33

 R11 R 21 R31
 R12 R 22 R32 
3 1
R

 0 

 R13 R 23 R33
(this is a transpose!)
Continuing:
U 11 U 12 U 13 
U 21 U 22 U 23 


U 31 U 32 U 33
 nx R11  n y R 21  nz R31 s x R11  s y R 21  s z R31 a x R11  a y R 21  a z R31 


 nx R12  n y R 22  nz R32 s x R12  s y R 22  s z R32 a x R12  a y R 22  a z R 32 
 nx R13  n y R 23  nz R33 s x R13  s y R 23  s z R33 a x R13  a y R 23  a z R33 


Finally:
C C C  S S
 S C C  C S


 S C
C C S  S C
 S C S  C C
S S
U 11 U 12 U 13 
U 21 U 22 U 23


U 31 U 32 U 33
C S 

S S 

C 
Solving for Individual Orientation Angles
(1st ):
• Selecting (3,3)→ C = U33
• With C we “know” S = (1-C2).5
• Hence:  = Atan2(U33, (1-U332).5
• NOTE: 2 solution for !
Re-examining the Matrices:
• To solve for : Select terms: (1,3) & (2,3)
 CS = U13
 SS = U23
 Dividing the 2nd by the 1st: S /C = U23/U13
 so Tan() = U23/U13
 leading to:  = Atan2(U13, U23)
Continuing our Solution:
• To solve for : Select terms: (3,1) & (3,2)
 -SC = U31
 SS = U32
 so Tan() = U32/-U31 (note the ‘minus’ sign track
the appropriate term!)
 And Thus  = Atan2(-U31, U32)
Summarizing:
•  = Atan2(U33, (1-U332).5
•  = Atan2(U13, U23)
•  = Atan2(-U31, U32)
Let’s examine a Spherical Wrist:
Z4
Y5
Y4
X4
X5
Y3
Z5
Y6
X3
Z3
X6
Z6
IKSing the Spherical Wrist
Frames
Link
Var

d
l

3→4
4
R
4
0
0
-90
4→5
5
R
5
0
0
+90
5→6
6
R
6
d6
0
0
C 4 0  S 4 
C 5 0 S 5 
C 6  S 6 0 
R4   S 4 0 C 4  ; R5   S 5 0 C 5 ; R6   S 6 C 6 0 






 0 1 0 
 0 1
 0
0 
0
1 
Writing The Solution:
C 4
S4

 0
U11
U
 21
U 31
0  S 4  C 5 0 S 5  C 6  S 6 0 





0 C 4  S 5 0 C 5  S 6 C 6 0 
 
 

1 0   0 1
0   0
0
1 
U12 U13 
U 22 U 23 

U 32 U 33 
Lets See (By Pure Inverse) Technique:
C 5
 S5

 0
 C4
 0

  S 4
0 S 5  C 6


0 C 5  S 6
 
1
0   0
S 4 0  U11


0 1 U 21

C 4 0  U 31
S 6 0

C6 0 

0
1 
U12 U13 

U 22 U 23

U 32 U 33 
Simplifying
C 5C 6 C 5S 6 S 5 
 S 5C 6  S 5S 6 C 5 


 S 6
C6
0 
C 4U11  S 4U 21 C 4U12  S 4U 22 C 4U13  S 4U 23 


U 31
U 32
U 33


C 4U 21  S 4U11 C 4U 22  S 4U12 C 4U 23  S 4U13 
Solving:
• After Examination here let’s select (3,3) both
sides:
•
•
•
•
0 = C4U23 – S4U13
S4U13 = C4U23
Tan(4) = S4/C4 = U23/U13
4 = Atan2(U13, U23)
• With the given and back-substituted values
(from the arm joints) we have a value for 4
the RHS is completely known
Solving for 5 & 6
• For 5: Select (1,3) & (2,3) terms
•
•
•
•
S5 = C4U13 + S4U23
C5 = U33
Tan(5) = S5/C5 = (C4U13 + S4U23)/U33
5 = Atan2(U33, C4U13 + S4U23)
• For 6: Select (3,1) & (3, 2)
•
•
•
•
S6 = C4U21 – S4U11
C6 = C4U22 – S4U12
Tan(6) = S6/C6 = ([C4U21 – S4U11],[C4U22 – S4U12])
6 = Atan2 ([C4U21 – S4U11], [C4U22 – S4U12])
Summarizing:
• 4 = Atan2(U13, U23)
• 5 = Atan2(U33, C4U13 + S4U23)
• 6 = Atan2 ([C4U21 – S4U11], [C4U22 – S4U12])
Lets Try One:
• Cylindrical Robot w/ Spherical Wrist
• Given a Target matrix (it’s an IKS after all!)
• The d3 “constant” is 400mm; the d6 offset (call
it the ‘Hand Span’) is 150 mm.
• 1 = Atan2((dx – ax*150),(dy-ay*150))
• d2 = (dz – az*150)
• d3 = ((dx – ax*150)2,(dy-ay*150)2).5 - 400
The Frame Skeleton:
F4
Z
X
F2.5 X
F2
F3 X
F5
X
Z
Z
X
Z
Z
X
F6
F1
F0
Z
Z
Z
X
X
Note “Dummy” Frame to
account for Orientation
problem with Spherical
Wrist
Solving for U:
C1  S1 0  0 0 1  0 0 1   0 1 0  nx ox ax 
U   S1 C1 0   1 0 0    1 0 0   1 0 0   ny oy a y 

 
 
 
 

 0 0 1   0 1 0   0 1 0  0 0 1   nz oz az 
NOTE: We needed a “Dummy Frame” to
account for the Orientation issue at the
end of the Arm
Simplifying:
U11 U12 U13  C1nx  S1nz

U

U 22 U 23   S1nx  C1nz
21


U 31 U 32 U 33  
ny
C1s x  S1s z
S1s x  C1s z
oy
C1a x  S1a z 

S1a x  C1a z 

ay

Subbing Uij’s Into Spherical Wrist Joint Models:
• 4 = Atan2(U13, U23)
= Atan2((C1ax + S1az), (S1ax-C1az))
• 5 = Atan2(U33, C4U13 + S4U23)
= Atan2{ay, [C4(C1ax+S1az) + S4 (S1ax-C1az)]}
• 6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12])
= Atan2{[C4(S1nx-C1nz) - S4(C1nx+S1nz)],
[C4(S1sx-C1sz) - S4(C1sx+S1sz)]}