ME 4135
Robotics & Control
R. Lindeke, Ph. D.
SLIDE SERIES 5:
IKS FOR ROBOTS
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 Geometry:
Cartesian
Space
Joint
Space
So this IKS is Nasty (as we
know!)
 It a more difficult problem because:
 The Equation set is “Over-Specified”
 12 equations in 6 unknowns
 Space is “Under-Specified”
 Planer devices with more joints than 2
 The Solution set can contain Redundancies
 Multiple solutions
 The Solution Sets may be un-defined
 Unreachable in 1 or many joints
Uses of IKS
 Builds Workspace
 Allows “Off-Line Programming” solutions
 Thus, compares Workspace capabilities with
Programming realities to assure that execution is
feasible
 Aids in Workplace design and operation
simulations
Performing IKS For (mostly)
Industrial Robots:
 First lets consider the previously defined
Spherical Wrist simplification
 All Wrist joint Z’s intersect at a point
 The nth Frame is offset from this intersection by a
distance dn along the a vector of the desired
solution (3rd column of desired orientation submatrix)
 This result is as expected by following 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
 … From the WRIST Joints
 Joints 4 to 6 in a full functioning spherical wrist
 Wrist Joints function as the 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
R Manipulator – a simple and
special case which can be found by
doing a ‘Pure IKS’ solution
X2
X1
Y0
Z1
Y1
Z2
Y2
X0
Z0
R Frame Skeleton
LP Table and Ai’s
Frames
Link
Var

d
a

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
ox
oy
oz
ax
ay
az
0
0
dx 
dy 

dz 

1
Forming The IKS:
 Examining these two equation (n, o, a and d are
‘givens’ in an inverse sense!!!):
 Term (1, 4) & (2,4) both side allow us to find an
equation for  as a positioning joint in the arm:
 (1,4): C1*(d2+cl2) = dx
 (2,4): S1*(d2+cl2) = dy
 Form a ratio to build Tan():
 S1/C1 = dy/ dx
 Tan  = dy/dx
  = Atan2(dx, dy)
Forming The IKS:
 After  is found, back substitute
and solve for d2:
 (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
ox
oy
oz
ax
ay
az
0
0
dx 
dy 

dz 

1
Simplify RHS means:
1
0

0

0
  S1 nx  C1 n y 

nz


  C1 nx  S1 n y 

0

0 0
0 
1 0
0 

0 1 d 2  cl2 

0 0
1 
  S1 o
C1 o
x
 C1 o y 
oz
x
 S1 o 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 (1,4)




0 = -S1*dx + C1*dy
Solving: S1*dx = C1*dy
Tan() = (S1/C1) = (dy/dx)
 = Atan2(dx, dy)
 Selecting and Equating (3,4) -- after back
substituting  solution
 d2 + cl2 = C1*dx + S1*dy
 d2 = C1*dx + S1*dy - cl2
Performing IKS For Industrial
Robots:
 What we saw with the -R IKS solutions will
always work for a robot – with or without a
spherical wrist
 These methods are often tedious – and
require some form of “intelligent application”
to find a good IKS solution
 Hence that is why we like the spherical wrist
simplification (for industrial robots)
introduced earlier – separating ARM and
Wrist Effects and joints
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)
 Where D =
2
2
2
2
2
P

P

P

a

a
 x y z 2 3
2a2 a3
 2 =  - 
  is: Atan2((Px2 + Py2).5, Pz)
  is:
A tan 2(sin 
cos 
)


A tan 2  Px2  Py2  Pz2  a22  a32  , 2a2a3 1  D 2 


One Further Complication:
 This can be considered the d2 offset problem
 A d2 offset is a design issue that states that
the nth frame has a non-zero offset along the
Y0 axis as observed in the solution of the T0n
with all joints at home
 This leads to two solutions for 1
 the So-Called Shoulder Left and Shoulder Right
solutions
Defining the d2 Offset issue
Here: ‘The ARM’ might be a revolute/prismatic device as in the Stanford Arm or it
might be a2 & a3 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
a3'
Z1
d2
F1
a2'
R'
a
X1
d2
Q11
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
6
3
 Separates Arm Joint and Wrist Joint
Contribution to the desiredTarget
(given) orientation
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 Arm
dependent!
 nx
n
 y
 nz
ox
oy
oz
ax 

ay

az  given
Simplifying the RHS: (our socalled 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 ox R11  o y R 21  oz R31 a x R11  a y R 21  a z R31 


n
R
12

n
R
22

n
R
32
o
R
12

o
R
22

o
R
32
a
R
12

a
R
22

a
R
32
y
z
x
y
z
x
y
z
 x

 nx R13  n y R 23  nz R33 ox R13  o y R 23  oz R33 a x R13  a y R 23  a z R33 


Finally:
C C C  S S
LHS  R36   SC C  C S


 S C

3 1
0
RHS   R  * Rgiven
CC S  SC
 SC S  CC
S S

C S 
S S  

C 
U 11 U 12 U 13 
 U 21 U 22 U 23


U 31 U 32 U 33
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 solutions 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
 Tan() = U23/U13
  = Atan2(U13, U23)
Continuing our Solution:
 To solve for : Select terms: (3,1) & (3,2)
 -SC = U31
 SS = U32
 Tan() = U32/-U31
  = Atan2(-U31, U32)
Summarizing:
  = Atan2(U33, (1-U332).5
  = Atan2(U13, U23)
  = Atan2(-U31, U32)
Let do a (true) Spherical
Wrist:
Z4
Y5
Y4
X4
X5
Y3
Z5
Y6
X3
Z3
X6
Z6
IKSing the Spherical Wrist
Frames
Link
Var

d
a

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:
 Examination here lets select (3,3) both sides:




0 = C4U23 – S4U13
S4U13 = C4U23
Tan(4) = S4/C4 = U23/U13
4 = Atan2(U13, U23)
 With the given desired orientation and 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 modeled earlier
Solving for U:
C1  S1 0   0 0 1  0 0 1   0 1 0   nx
U   S1 C1 0    1 0 0    1 0 0   1 0 0    n y

 
 
 
 
 0
0 1   0 1 0   0 1 0   0 0 1   nz
NOTE: We needed a “Dummy
Frame” to account for the Orientation
issue at the end of the Arm
ox
oy
oz
ax 
ay 

az 
Simplifying:
U11 U12 U13  C1nx  S1nz
U
   S1n  C1n
U
U
22
23 
x
z

 21
U 31 U 32 U 33  
ny
C1ox  S1oz
S1ox  C1nz
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(S1ox-C1oz) - S4(C1ox+S1oz)]}