Chapter 2

Math Fundamentals

Part 2
4. Kinematics of Mechanisms
5. Orientation and Angular Velocity
 Outline
• 2.4 Kinematics of Mechanisms
• 2.5 Orientation and Angular Velocity
 Outline
• 2.4 Kinematics of Mechanisms
– 2.4.1 Forward Kinematics
– 2.4.2 Inverse Kinematics
– 2.4.3 Differential Kinematics
– Summary
• 2.5 Orientation and Angular Velocity
 Definitions
• Motion = movement of the whole body thru
space.
• Attitude = pitch and roll.
• Orientation = attitude & (heading or yaw).
• Pose = position & orientation
 Task and Joint Space
• For manipulators, 2

joint space is also C

Space. 1

• Manipulators are 3
Joint Space
difficult for operators
to control in joint
space.

BASE TOOL

Task Space
2.4.1.2 Mechanism Models
 2.4.1.2 Mechanism Models
• Let  represent the articulations of a mechanism.
• It is convenient to think about the moving axis operations
which align a sequence of frames with each other.
2.4.1.2 Mechanism Models
 2.4.1.3 Denavit-Hartenberg Convention
• A special product of 4 fundamental operators is
used as a basic conceptual unit.
• Its still an orthogonal transform so it has the
properties of the component transforms, namely:
– Operates on points
– Converts coordinates
– Represents axes of one system wrt another
 Denavit- Hartenberg Convention
• Move first frame into coincidence with second thus:
– rotate around the xk-1 axis by an angle k
– translate along the xk-1 axis by a distance uk
– rotate around the new z axis by an angle k
– translate along the new z axis by a distance wk
 Denavit- Hartenberg Convention
• Moving axis operations in left to right order:
T kk – 1 = R otx  k  Tr ans uk  0 0 Rotz  k  Tr a ns0 0 wk 
 Denavit- Hartenberg Convention
c k –s k 0 uk
c k s k c k c  k –s k –s k w k
A k = T kk – 1 =
s s s c  c k
k k k k
c k w k

0 0 0 1
Special
Notation

• This matrix has the following interpretations:

– It will move a point or rotate a direction by the operator which
describes how frame k is related to frame k-1.
– Its columns represent the axes and origin of frame k expressed in
frame k-1 coordinates.
– It converts coordinates from frame k to frame k-1.
Three Link Planar Manipulator
2.4.1.4 Example: 3 Link Planar Manipulator
y0,1 y2
0 y3 y4
x0,1 x2 3 x4
1
1
2
2 3 x3

L1 L2 L3

Link  u  w
0 0 0  0
1 0 L1  0
2 0 L2  0
3 0 L3 0 0
 Example: 3 Link Planar Manipulator
c k –s k 0 uk
c k s k c k c  k –s k –s k w k
A k = T kk – 1 =
s s s c  c k
k k k k
c k w k

0 0 0 1

c1 –s 1 0 0 c1 s1 0 0 c 2 –s2 0 L 1 c 2 s 2 0 –c 2 L 1
s 1 c1 0 0 –1 –s 1 c 1 0 0 s2 c2 0 0 –s 2 c2 0 s 2 L 1
A1 = A1 = A2 = A –1
2 =
0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1

c3 –s 3 0 L 2 c3 s 3 0 –c3 L 2 1 0 0 L3 1 0 0 –L3
s 3 c3 0 0 A –1 –s 3 c3 0 s 3 L2
A3 = 3 = A4 = 0 1 0 0 A4– 1 = 0 1 0 0
0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1
 Example: 3 Link Planar Manipulator

c 123 –s123 0  c L + c L + c L 
123 3 12 2 1 1
s123 c123 0  s L + s L + s L 
T40 = 123 3 12 2 1 1
0 0 1 0
0 0 0 1
orientation position
A Completely General Forward Kinematics Solution!
 Outline
• 2.4 Kinematics of Mechanisms
– 2.4.1 Forward Kinematics
– 2.4.2 Inverse Kinematics
– 2.4.3 Differential Kinematics
– Summary
• 2.5 Orientation and Angular Velocity
 Kinematics

Tool

Base

Shoulder Elbow Wrist

Tool
Base
 2.4.2.1 Existence and Uniqueness
• Existence
– Nonlinear equations need not be solveable.
• Uniqueness
– There is no rule requiring only one answer.
 Existence
• Nonlinear equations need not be solveable.

Goal
 Uniqueness
• There is no rule requiring only one answer.

Goal
 2.4.2.2 Technique
• Rewrite equations in multiple ways in order to isolate unknowns.
– No new info generated, its just algebra.
 2.4.2.3 Example: 3 Link Planar Manipulator
• Account for known quantities.
• This is a 2D problem, so…

r11 r 12 r 13 px r11 r 12 0 p x
r r r p r21 r 22 0 p y
T 40 = 21 22 23 y =
0 0 1 0
r31 r 32 r 33 pz
0 0 0 1
0 0 0 1
 Example: 3 Link Planar Manipulator
T 40 = A 1 A2 A 3 A 4
r11 r12 0 p
x
c 123 –s
123 0  c123 L3 + c 12 L 2 + c 1 L1 
r21 r22 0 p
y =
s123 c
123 0  s123 L 3 + s 12L 2 + s 1 L 1 
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

• From the (1,1) and (2,1) elements we have:

 123 = atan 2 r 21  r11 

 Example: 3 Link Planar Manipulator
T 40 A 4–1 = A1 A2 A 3
r11 r 12 0 – r L + p c123 –s 123 0 c1  c2 L 2 + L 1  – s 1  s 2 L2 
11 3 x
r21 r 22 0 – r L + p
21 3 y =
s 123 c123 0 s1 c 2 L2 + L1  + c1  s 2 L 2 
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

r11 r 12 0 – r 11L 3 + p x c123 –s 123 0 c12 L + c L

2 1 1

r21 r 22 0 – r 21L 3 + p y = s 123 c123 0 s L + s L

12 2 1 1
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

• From the (1,4) and (2,4) elements we have:

k1 = – r 11L 3 + p x = c 12 L2 + c1 L1

k 2 = – r 21L 3 + p y = s 12 L 2 + s 1 L 1
 Example: 3 Link Planar Manipulator
• Repeat from last slide: from the (1,4) and (2,4) elements we have:
k1 = – r 11L 3 + p x = c 12 L2 + c1 L1

k 2 = – r 21L 3 + p y = s 12 L 2 + s 1 L 1
• Square and add (eliminates 1) to yield:

k12 + k22 = L 22 + L 12 + 2 L2 L 1 c 1 c 12 + s 1 s 12 
• Or:
k 12 + k22 = L 22 + L12 + 2L 2 L 1 c 2 cos2
• Rearranging gives the answer:

• Each solution gives different values for the other two angles.
 Example: 3 Link Planar Manipulator
• 2nd time, from the (1,4) and (2,4) elements we have:
k1 = – r 11L 3 + p x = c 12 L2 + c1 L1

k 2 = – r 21L 3 + p y = s 12 L 2 + s 1 L 1
• With 2 known, these are:
c 1 k3 – s 1 k4 = k1
s 1 k3 + c 1 k 4 = k 2
• That’s a standard form. The solution is:

• The last angle is then:

 That’s PRETTY AWESOME!
 12 3 = ata n 2 r 21  r11 
k12 + k 22  – L 22 + L 12
2 = acos ----------------------------------------------------
2L 2 L1
 1 = atan 2  k2 k3 – k1 k 4   k1 k3 + k 2 k4  Goal
 3 =  123 –  2 –  1
 2.4.2.4 Standard Forms: Explicit Tangent
• Occurs very frequently:
a = cn
b = sn
b
• Isolate tangent: a

• Clearly:  n = atan2b a

Why do we need two equations for one unknown?

 2.4.2.4 Standard Forms: Point Symmetric
• Occurs as:
sna – c nb = 0
• Isolate tangent:
b
a

• Clearly:
 n = atan2b a 
Why two solutions?
n = atan 2–b –a
 2.4.2.4 Standard Forms: Line Symmetric
• Occurs as:
sna – c n b = c
• Trig substitution: r
a = r cos   b
b = r sin   a
• Leads to:
s  –  n  = c  r
c  –  n  = s qr t1 –  c  r  2 
• So:
• Solution
 n = atan 2  b a  – atan 2  c s q r t r2 – c 2  
 Outline
• 2.4 Kinematics of Mechanisms
– 2.4.1 Forward Kinematics
– 2.4.2 Inverse Kinematics
– 2.4.3 Differential Kinematics
– Summary
• 2.5 Orientation and Angular Velocity
 Differential Kinematics
• Studies the derivatives (first order behavior) of
kinematic models.
• Use Jacobians (multidimensional derivatives) to
do this.
• Use them for:
– Resolved rate control
– Sensitivity analysis
– Uncertainty propagation
– Static force transformation
 Derivatives of Fundamental Operators
1 0 0 0
R otx  = 0 c –s 0
0 s c 0
x 0 0 0 1

0 0 0 0
 R otx  =
0 –s –c  0
 0 c  –s 0
0 0 0 0
 z

y
Derivatives of Fundamental Operators
 2.4.3.2 Mechanism Jacobian
• How much does end effector move if I tweak this
angle 1 degree?
 2.4.3.2 Mechanism Jacobian
• Recall a Jacobian….
x and q can
• If:
be anything

• Then:

• Basic use:
 Resolved Rate Control
• By the Chain Rule of Differentiation:

• This is just:

• Suppose x is the position of the end effector. This

is:
What does
– Nonlinear in joint variables that imply?
– Linear in the joint rates!
 Resolved Rate Control
v

• Direct mapping from desired velocity onto joint rates!

• Singularity (infinite joint rates) occurs:

– at points where two different inverse kinematic solutions
converge
– when joint axes become aligned or parallel
– when the boundaries of the workspace are reached
 2.4.3.4 Example: 3 Link Planar Manipulator
• Forward kinematics:

• Differentiate wrt 1, 2, and 3.

·

• In matrix form:
 Outline
• 2.4 Kinematics of Mechanisms
– 2.4.1 Forward Kinematics
– 2.4.2 Inverse Kinematics
– 2.4.3 Differential Kinematics
– Summary
• 2.5 Orientation and Angular Velocity
 Summary
• We can model the forward kinematics of
mechanisms by
– embedding frames in rigid bodies
– employing the fundamental orthogonal
operator matrices
– employing a few rules for writing them in the
right order to represent a mechanism.
• The DH convention consists of a special
compound orthogonal transform and a few
rules for orienting the link frames.
– These can be used just like the fundamental
orthogonal operator matrices to do forward
kinematics.
 Summary
• Inverse kinematics requires more skill and
relies on rewriting the forward equations in
an attempt to isolate unknowns.
• Various derivatives of kinematic transforms
can be taken and each has some use.
 Outline
• 2.4 Kinematics of Mechanisms
• 2.5 Orientation and Angular Velocity
– 2.5.1 Orientation in Euler Angle Form
– 2.5.2 Angular Rates and Small Angles
– 2.5.3 Angular Velocity and Orientation Rates in Euler
Angle Form
– 2.5.4 Angular Velocity and Orientation Rates in Angle-
Axis Form
– 2.5.5 Summary
 Definitions
• Yaw  = rotation about vertical axis
• Pitch  = rotation about sideways axis
• Roll  = rotation about forward axis.
• Attitude = roll & pitch [ ]
• Orientation = attitude + yaw [  ]
• Pose = position + orientation [x y z   
• Azimuth = yaw (for a pointing device)
• Elevation = pitch (for a pointing device)
• Heading = angle of path tangent. Sometimes
same as yaw. Sometimes not.
 Definitions
• Pose = position & orientation

• In 2D:

• In 3D:
 2.5.1.1. Axis Conventions
• Aerospace:

x
roll
y
pitch z
yaw

• Ground Vehicles (here at least): z

y
pitch
yaw
x
roll
2.5.1.2 Frame Assignment
Letter Name

n,w Navigation, world

w world

p positioner

b,v body, vehicle

h head

s sensor

c wheel contact
 2.5.1.3 The RPY Transform
• Similar to DH Matrix but encodes 6 dof:
• Aligning operations that move ‘a’ into coincidence
with ‘b’:
– translate along the (x,y,z) axes of frame ‘a’ by (u,v,w)
until its origin coincides with that of frame ‘b’
– rotate about the new z axis by an angle  called yaw
– rotate about the new y axis by an angle  called pitch
– rotate about the new x axis by an angle  called roll
 RPY Forward Kinematics
Tba = Tr ans u v w R otz Roty  Rotx 

1 0 0 u c –s 0 0 c 0 s 0 1 0 0 0
T ba = 0 1 0 v s c  00 0 1 0 0 0 c –s 0
0 0 1 w 0 0 1 0 –s 0 c 0 0 s c 0
0 0 0 1 0 0 01 0 0 0 1 0 0 0 1

c c  c s s – sc   c sc + ss  u

T ba = sc   sss + c c   ss c – cs v
–s  c s c c w
0 0 0 1
 2.5.1.4 RPY Transform Inverse Kinematics
T ba = Tra n su  v  w  Ro tz  R o t x  R o t y  

r11 r12 r13 p x

c c c ss – sc   csc  + ss u
= sc   sss + c c  ssc – c s
r21 r22 r23 p y v
r31 r32 r33 p z –s c s c c  w
0 0 0 1 0 0 0 1

body x axis  = atan 2  r21 r 11 

in world
• Yaw can be determined from a vector aligned with
the body x axis expressed in world coordinates .
• Is there another solution?
• Only if there are 2 solutions for .
2.5.1.4 RPY Transform Inverse Kinematics
 R otz  –1  Tr ans u v w–1 T ba = Roty  Rotx 

 r11 c + r21 s  r12 c + r22 s  r13 c + r23 s 0 c  ss sc 0
 –r 11s + r21 c –r 12s + r22 c –r13 s + r23 c  0 0 c –s 0
=
r31 r32 r33 0 –s c s c c  0
0 0 0 1 0 0 0 1

body x axis wrt yawed frame

 = atan 2 – r31 r 11c + r 21 s 
2.5.1.4 RPY Transform Inverse Kinematics
R otx  – 1 R otz   – 1 Tr ans u v w  – 1 T ba = Roty  

. cr 12c  + r 22 s  – r32 s . 0 1 0 0 0

. – r12s + r22 c  . 0 c –s
= 0 0
. s r12 c  + r22 s + r32 c . 0 0 s c 0
01 0 0 0 1
0 0

 = atan 2 s r 1 2c  + r 22 s  + r32 c – r1 2 s + r22 c 

 Outline
• 2.4 Kinematics of Mechanisms
• 2.5 Orientation and Angular Velocity
– 2.5.1 Orientation in Euler Angle Form
– 2.5.2 Angular Rates and Small Angles
– 2.5.3 Angular Velocity and Orientation Rates in Euler
Angle Form
– 2.5.4 Angular Velocity and Orientation Rates in Angle-
Axis Form
– 2.5.5 Summary
 2.5.2.1 Euler’s Theorem
• All rigid body rotations in 3D can be represented as a
single rotation by some amount (angle) about a fixed
axis:

R ot        ̂  

=
 2.5.2.2 Rotation Vector Representation
• Idea: Use a unit vector scaled by
the magnitude of the rotation:
T
 = x y z

• The axis of rotation is:

̂ =   
 2.5.2.3 Relationship to Angular Velocity
• Suppose the rotation angle is
differentially small.
T
d  = d x d y d  z

• The angular velocity is such that:

d- ----------
 = --------- d  = d
ˆ = ̂
dt  d 
• This is a true vector in the linear
algebra sense.
• Conversely, the rotation vector is:
T
d =  dt = x y z dt
 Recall the Rot() Operators
x y z
p’ p’ p’
p p p

z x y
  
y z x
1 0 0 0 c 0 s 0 c – s  0 0
R otx  = 0 c –s 0 Roty   = 0 1 0 0 R o t z   = s c  0 0
0 s c 0 –s 0 c 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
 2.5.2.4 Rotations Through Small Angles
• For small angles:
sin    
cos   1

• Substituting into the Rot() operators:

1 0 0 0 1 0  0 1 – 0 0

R otx  = 0 1 – 0 Roty   = 0 1 0 0 Rotz   =  1 0 0

0  1 0 – 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

• Useful for computing rapid differentials of complicated

kinematic expressions.
• Turns out that while 3D rotations do not commute,
differential 3D rotations do commute.
 General Differential Rotations
• Consider a 3D composite differential rotation:
T
 =   

• Substituting into the differential Rot() operators and

cancelling H.O.T:
1 –  0
RotxRotyRotz  = Rot =  1 – 0
–  1 0
0 0 0 1

• To first order, the result does not depend on the order

the rotations are applied.
 2.5.2.4 Skew Matrices
• Last result can be written in terms of a skew matrix:

Where:
Beware:
TYPO in book!
This is correct
 Outline
• 2.4 Kinematics of Mechanisms
• 2.5 Orientation and Angular Velocity
– 2.5.1 Orientation in Euler Angle Form
– 2.5.2 Angular Rates and Small Angles
– 2.5.3 Angular Velocity and Orientation Rates in Euler
Angle Form
– 2.5.4 Angular Velocity and Orientation Rates in Angle-
Axis Form
– 2.5.5 Summary
 2.5.3.1 Relation to Euler Angle rates
• The pitch , yaw , and roll  angles are
called Euler angles.
• Their rates are not exactly the angular
velocity vector.  x
 = y

d z
dt 
 measured about
fixed axes

measured about z
moving axes
y
x
 Relation to Euler Angle rates
• Use Chain Rule. Convert coordinates for each
component into the body frame. Note use of
transform matrices (from world to body).

Why express  in the body frame?

 Inverted form
• Most useful in inverted form:

· x +  y st +  z ct 1 s t c t 
z  x
· =  y c – z s = 0 c –s 
 y
y
·  s---
y -- +  c-----
z
s
0 -----
c
----- 
x c c c c z

 z c +  y s = c
·
• because:
• because:  y c –  z s = ·

Why convert  to Euler Angle Rates?

 Outline
• 2.4 Kinematics of Mechanisms
• 2.5 Orientation and Angular Velocity
– 2.5.1 Orientation in Euler Angle Form
– 2.5.2 Angular Rates and Small Angles
– 2.5.3 Angular Velocity and Orientation Rates in Euler
Angle Form
– 2.5.4 Angular Velocity and Orientation Rates in Angle-
Axis Form
– 2.5.5 Summary
 2.5.4.1 Angular Velocity as a Skew Matrix
• Recall the skew matrix formed from such a
small 3D rotation.
0 –dz d y 0
S kew d = d X = d z 0 –d x 0
–d y d x 0 0
0 0 0 0

• Dividing by dt permits an equivalent matrix

to be formed from the angular velocity
vector: This is very closely related
0 – z y 0
to the derivative of a
 = Skew d = X z 0 –x 0 rotation matrix.
 -------
dt 
=
– y  x 0 0
0 0 0 0
2.5.4.2 Time Derivative of a Rotation Matrix
n
• Let the matrix R track the orientation of a
k
moving frame k with respect to frame n.
• For small time steps, the update to time k+1
is a composition with a small perturbation:
k+1
k
• By definition of derivative:
n
n n n
·n R k Rk t +  t – Rk t 
R k = lim --------- = lim ------------------------------------------------
t  0  t t  0 t
2.5.4.2 Time Derivative of a Rotation Matrix
• Recall from last slide:
n n n
·n R k Rk t +  t – Rk t 
R k = lim --------- = lim ------------------------------------------------
t  0  t t  0 t

• Write this in terms of a perturbation. k+1

k
n
• Hence, since R is fixed: k n
·n n X
Rk   n  X
R k = lim ---------------------- = R k lim ----------------
t  0 t t  0 t

• Hence: ·n n  d  
X
n
Rk = R k   = R k k
 dt 
 Time Derivative of a Rotation Matrix
• In other words, we have the remarkable
result that:

k+1
• This holds so long as the components of k
the skew matrix are expressed in the
moving frame k: n
0 – z y 0 Why?
= S kew k n  = z 0 – x 0
k
k
The perturbation
–y x 0 0 was expressed as a
rotation matrix
0 0 0 0 composition.
 Outline
• 2.4 Kinematics of Mechanisms
• 2.5 Orientation and Angular Velocity
– 2.5.1 Orientation in Euler Angle Form
– 2.5.2 Angular Rates and Small Angles
– 2.5.3 Angular Velocity and Orientation Rates in Euler
Angle Form
– 2.5.4 Angular Velocity and Orientation Rates in Angle-
Axis Form
– 2.5.5 Summary
 Summary
• The RPY matrix is yet another compound orthogonal
operator matrix. Unlike the DH matrix, it has 6 dof, so it is
completely general.
• Angular velocity is the time derivative of the rotation
vector.
• Compositions of small angle rotations behave
commutatively.
• Angular velocity and small rotations can be expressed as
skew matrices.
• Angular velocity is related in a complicated manner to the
rates of roll, pitch, and yaw angles.
• The skew of angular velocity is related in a more elegant
manner to the rate of the rotation matrix.