Robot Kinematics

Kinematics studies the motion of

bodies
 We are interested in two kinematics topics

Forward Kinematics (angles to position)

What you are The length of each link
given: The angle of each joint

What you can find: The position of any point

(i.e. it’s (x, y, z)
coordinates
Inverse Kinematics (position to angles)
What you are The length of each link
given: The position of some point on the
robot
What you can find: The angles of each joint needed
to
Obtain that position
 Quick Math Review
Dot Product:  a x
Geometric Representation:  
 a y

AB  A B A bx
 
cos θ θ  b y
B
Matrix Representation:
ax bx
AB      a xb x  a yb y

ay by

Unit Vector
Vector in the direction of a chosen vector but whose magnitude
is 1. B
uB
 B B

uB
 Quick Matrix Review

Matrix Multiplication:
An (m x n) matrix A and an (n x p) matrix B, can be multiplied
since the number of columns of A is equal to the number of rows of
B.
Non-Commutative Multiplication
AB is NOT equal to BA
f   ae  bg  af  bh 
 a b     e
 
c d g

c  dg 
cf  dh 
   h 
e

Matrix Addition:
a b    e
f   a  e b  f 
c d g   
   h   c  g  d  h  
 Basic Transformations
Moving Between Coordinate Frames

Translation Along the X-

Axis O
Y
(VN,VO)

VO

P
N
X
VN
P
x
Px = distance between the XY and NO coordinate planes

P x

Notation: VX VN
V
XY
  V
NO
  P  
Y O
V V 0
 Writing V
XY in terms o fV NO

Y O

VO
P 
VN N
X

 P  V N
XY  X   P NO
V V
O
 V 
 O
Translation along the X-Axis and Y-
Axis
Y

VO

N
VN

 Px 
X P
XY 
 Y  
P

PX VN
V
XY P V NO

P  V
O 
 Y 
 Using Basis
Vectors
Basis vectors are unit vectors that point along a
coordinate
axis
O
n Unit vector along the N-Axis
o Unit vector along the O- Axis
VO
V
NO Magnitude of the VNOvector
o
N
n VN

 V NO  NO cos θ 
VN cos θ  V  V NO n
NO
  
cos(90    V
V  O NO NO NO
 V  V o
 V    θ)   
sin θ 

 Y
Rotation (around the Z-Axis)
Z X
Y

VY


X
V
X

 = Angle of rotation between the XY and NO coordinate

axis
VX VN
V
XY
  V
NO
 
Y O
V V
 Y
Unit vector along X-Axis
Can be considered with respect to
the XY coordinates or NO
V coordinates XY NO
V  V

VY


X
VX
X XY NO NO
V  V cos α  V cos α  V x

X N O
(Substituting for VNO using the N and O
V  (V nV o)x components of the vector)
X N O
V  V (x  n)  V (x  o)

 V N (cos θ )  V O (cos( θ  90))

 V N (cos θ )  V O (sin θ )
Similarly…
. NO NO NO
V
Y  V sin α  V cos(90  α) y
 V
Y N

O
V  (V  n  V  o )  y
Y N O
V  V (y  n)  V (y  o)

 V N (cos(90  θ))  V O (cos

θ)

 V N
(sin θ )  V O (cos θ )

X
So….N
V  V (cos θ)  V
O
(sin θ) VX
V
XY
 Y 
V
Y
 V
N
(sin θ)  V
O
(cos θ) V


Written in Matrix
Form
VX  cos  sin θ   V N  Rotation Matrix about the
V
XY
 θ Y     O  z-
V
  sin θ cos θ   V  axis
 Y1

(VN,VO)
Y0

VNO

VXY

P X1

Translation along P followed by rotation by


X0

V X
   P x   cos θ  sin θ   V N 
XY
     
V  P
Y  O
V  y  sin θ cos θ   V 

(Note : Px, Py are relative to the original coordinate frame. Translation followed by
rotation is different than rotation followed by translation.)

In other words, knowing the coordinates of a point (V N,VO) in some

coordinate frame (NO) you can find the position of that point relative to your
original coordinate frame (X0Y0).
 HOMOGENEOUS REPRESENTATION
Putting it all into a Matrix

What we found by doing a

V X
 P x
  cos θ  sin θ   V N 
XY
     translation and a rotation
V  P
V
Y
 
sin θ cos θ O

 y  V


V  X
Px
  cos θ  sin θ 0V
N

  Padding with 0’s and 1’s
 V Y  
P sin θ cos θ 0 
O

   V
 y
  
 1  1 
  0 1  1  
0  

V X
  sin θ Px V N

 cos θ 
   cos θ P  V
O  Simplifying into a matrix form
  V Y  sin
 θ y 

 1  0 
   0 1   
1


 cos θ  sin θ Px

H

  sin θ
Homogenous Matrix for a Translation in

cos θ P y
XY plane, followed by a Rotation
 0 1  around the z-axis
0
 Rotation Matrices in 3D – OK,lets return from
homogenous repn
 cos θ  sin θ 0 Rotation around the Z-
  Axis
R z   sin θ cos θ 0 0 

 0  1


 cos θ 0 sin θ 
R  Rotation around the
y    0 0

1 Y-
  sin θ cos θ  Axis
0

0 0 
1
 Rotation around the X-
Rz   0 cos θ
Axis

 0  sin θ cos θ 
sin θ
 Homogeneous Matrices in 3D
H is a 4x4 matrix that can describe a translation, rotation, or both in
matrix
one O

Y 1 0 0

Px
N
P  
z
X A 1P 0
y

Translation without
Z rotation
0 0 1
Y nx ox Pa 0
x

O   0y oy 0a y 00
n 
N H  1 
n z
oz az 0
 
X
 0 0 0
1
Rotation part:
Z Rotation without Could be rotation around z-
A axis, x-axis, y-axis or a
translation 97
combination of the three.
 Homogeneous Continued….

VN
 O
V The (n,o,a) position of a point relative to the
 current coordinate frame you are in.
H
XY
V
VA
 1



n x ox ax Px   V N 
 O
 P V  V
X n xVN o xVO a xVA P
n y
oy ay x

V
XY
  y 

n Pz   A 
z
oz az 
V 

1 
0 0 0 1  

The rotation and translation part can be combined into a single

homogeneous matrix IF and ONLY IF both are relative to the same
coordinate frame.
98
 Finding the Homogeneous Matrix
EX.

Y W

N

O 
W 
 W A

T
P
X

Z
I N
W  W 
W

X

 Point relative to the  
J
Point relative to the  O  Point relative to the
Y
X-Y-Z frame W
 I-J-K frame W  N-O-A frame
W W K
  W A 
 Z
  

W

 
N

Pi   W
I
I   W  n i oi ai
W  Pi i n i o i
W 
N
 J   O
   W  P W
a n j o j a j 
 W
J j  W O
    j
 
j    j

W  P  n o Pk  
K
 a W A
 W K  n k ok a k W
A

 j Pk  n k k
k     
 1  0 0 0  1
o a   1 99
 Y
N
W 
 O 
W 

 W 
A

T
P
X

W X ix jx kx Tx   W I 
W 
X  kx W 
I    J
Tx ix jx Y

    W iy W 
 
 W    T y   i y
Y  J    j ky Ty y 
j y ky  W 
WZ   W Z j i z z W K

kz Tz
    
 T z   i z k z  W  
k
 j z
 1  
0 0 0 1   1



N
W X Tx oi ai Pi   W 
i x jx k x n i
W  I
 Y  
   O
Substituting iy jy ky Ty nj oj aj Pj W 
J

W  W     
for  W K

 W Z jz k Tz   n ok ak Pk  
iz W 
A
 z k
    
  1   1
0 0 0 1 0 0 0 1  

 W
X
  W N Tx oi ai Pi 
i x jx kx n i
 Y    
O H iy jy ky Ty n j

oj aj Pj

 iz jz Tkz   n ok ak Pk 
W H z k
  
1 1
W
  0 0 0 1  0
 W Z  W A Product of0the two 0 1
   matrices
Notice that H can also be written
as:  
1 0 0 Tx   i x jx kx 01 0 0 Pi   n i
    oj oi ai 
10 0 T y
 i y y y 0 0 j
 n j j 0
H  a 0

z   i z z z k n k k k
0 0 1 T  o
 1 0 j a k

0 0 0
1 0

H = (Translation relative to the XYZ frame) * (Rotation relative

P

to the
XYZ frame)
* (Translation relative to the IJK frame) j* (Rotation relativekto the
0   0IJK
frame) 0 1
P 0
  
0 0 10
 The Homogeneous Matrix is a concatenation of
numerous translations and rotations

Y
N
W 
 O 
W 
 W A


T
P
X

Z
One more variation on finding H:

H= (Rotate so that the X-axis is aligned with T)

* ( Translate along the new t-axis by || T || (magnitude of T))
* ( Rotate so that the t-axis is aligned with P)
* ( Translate along the p-axis by || P || )
* ( Rotate so that the p-axis is aligned with the O-axis)

This method might seem a bit confusing, but it’s actually an easier
way to solve our problem given the information we have.
 HOMOGENEOUS COORDINATES
For vectors in Cartesian coordinates, the basis is a set of unit vectors
directed along the orthogonal x, y, and z axes used to represent the
space. Thus, a vector v can be written as

where the unit vectors {i, j, k} have been weighted by the appropriate
constants
Another way of representing a three-dimensional vector is by
using homogeneous coordinates. In this representation a fourth
component which acts as a scaling factor is added.

Where ,

and s is the scaling factor.

The physical coordinates are obtained by dividing each component in

the homogeneous representation by the scale factor.

If the value of the scaling factor s is set to 1, the components of the

homogeneous and standard Cartesian representations are
identical. The importance of the fourth element will become clear
when we introduce matrix operators.
 VECTOR REPRESENTATIONS

Cartesian basis representation:

Homogeneous representations:

Three of the many possible column matrices which could

represent v are shown.

Each of these column matrices represents the

same vector in three-dimensional space.
When the scaling factor s is set to 0, the vector in homogeneous
coordinates represents a direction.
As s approaches zero, the vector components become infinite. Thus,
direction vectors pointing along the x, y, and z axes can be represented as
follows:

The null vector representing a point at the origin of a reference frame is

defined as

where s, the scaling factor, can be any number other than zero. We will
usually use scaling factors of 1 for our work. Thus the null vector will also use
a scan factor of 1. Physically, the null vector represents the origin of a
coordinate reference frame. A vector is undefined if all the entries are zero.
DOT PRODUCT EQUATIONS

Obtain the Cartesian and homogeneous representations for the dot product
of the two vectors x and y.

Cartesian basis representation:

Then,
Homogeneous representation:
The same vectors represented in homogeneous coordinate are
given by

where:
Defines the dot product in terms
of all the column entries by:
CROSS-PRODUCT FORMULAS

Obtain the Cartesian and homogeneous representations for the cross product
of the two vectors x and y

Cartesian basis representation:

The vector cross product is defined as

Homogeneous representation:

By substituting the coefficients (a' through f` )

Definition of the vector cross product the
following representation is obtained:

where:
 Matrix Operators: Transformations

In addition to performing the vector operations outlined previously, it is possible

to define a class of matrix operators that can perform simultaneous vector
operations resulting in the translation or rotation of a vector. The matrix H will
be considered such an operator, which transforms a point or vector x into
another point or vector y.

Translational transformations

The operator that moves the point defined by the original

vector x to a new point y whose location is given by the
vector addition of x and a translation vector defined by
the components a, b, and c.
The operator transforms the coordinates of the original vector
into the coordinates of a new vector. This matrix operator is
defined as follows:
Suppose that a point p lies at (x, y) = (5, 5) in a two-dimensional
reference frame. lt is desired to move the point along the diagonal
corresponding to an angle of 45° for a distance of 10 units. What are
the coordinates of the final point p' (x2. y2)?

The original point may be represented as a

vector in homogeneous coordinates. Its
value is

Since it is desired to move the point along

the 45° radial extension, the unit vector
corresponding to this direction is
To get a motion of 10 units it is necessary to multiply the components
or u by 10, excluding the scaling factor, The new vector u becomes

That can be considered a point 10 units from the origin along the
radial line at 45°. we obtain the expression for translated point p'

Carrying out the multiplication results:

 Rotational transformations

The x-y plane

Combining these equations yields the

desired results:
The derivation in the x-y plane is easily extended to either the y-z or
z-x plane. Rotational transformations which operate on homogeneous
coordinates and perform rotations about a given axis of the reference
coordinate system can be
 ROTATION OF A POINT ABOUT A REFERENCE FRAME
It is desired to rotate the point u represented by a column vector about the
z axis and determine the coordinates in terms of the original reference
frame. Figure shows this operation. The vector u will be given as
where v1, v2, v3 represents a vector to the rotated point after 60°, 90°,
and 120° of rotation, respectively.
 FORWARD
KINEMATICS
 The Situation:
You have a robotic arm that
starts out aligned with the xo-
axis.
You tell the first link to move by
1 and the second link to move
by 2.

The Quest:
What is the position of
Solution:
the end of the robotic arm?
1. Geometric
This might be the easiest solution for the simple situation.
Approach
However, notice that the angles are measured relative to the
direction of the previous link. (The first link is the exception.
The angle is measured relative to it’s initial position.) For
robots with more links and whose arm extends into 3
dimensions the geometry gets much more tedious.

2. Algebraic Approach
Involves coordinate transformations.
Example Problem:
You are have a three link arm that starts out aligned in the x- axis.
Each link has lengths l1, l2, l3, respectively. You tell the first one to
move by 1 , and so on as the diagram suggests. Find the
Homogeneous matrix to get the position of the yellow dot in the X0Y0
frame. Y 3

3

2 X3
3
Y2 2
X2

H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 )

Y0
i.e. Rotating by 1 will put you in the X1Y1 frame.
1
Translate in the along the X1 axis by l1.
1
Rotating by 2 will put you in the X2Y2 frame.
X0
and so on until you are in the X3Y3 frame.

The position of the yellow dot relative to the X3Y3 frame is (l1,
0). Multiplying H by that position vector will give you th
coordinates of the yellow point relative the the X0Y0 frame
 Slight variation on the last solution:
Make the yellow dot the origin of a new coordinate X4Y4 frame
Y3

Y4
3

2 X3
3
Y2 2
X2
X4

H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 ) * Tx3(l3)

Y0 1
This takes you from the X0Y0 frame to the X4Y4
frame.
1
X0
The position of the yellow dot relative to the X4Y4
frame is (0,0).
X 0
   
Y 0
 H  Notice that multiplying by the (0,0,0,1) vector

  0
will equal the last column of the H matrix.
 1 
  106
Z 1
More on Forward Kinematics…

Denavit - Hartenberg
Parameters
Denavit-Hartenberg Notation
Frame Assignment
 Denavit-Hartenberg Notation

Z(i - 1)

Yi Zi
Y(i -1)

Xi ai
a(i - 1 ) di
X(i -1)
i
( i - 1)

IDEA: Each joint is assigned a coordinate frame. Using the

Denavit-Hartenberg notation, you need 4 parameters to
describe how a frame (i) relates to a previous frame ( i -1 ).

THE , a , d,
PARAMETERS/VARIABLES: 
 The Parameters

You can
Z(i - 1) align the
Yi two axis just
Y(i -1) Zi
using the 4
Xi ai parameter s
a(i - 1 ) di
X(i -1)
i
( i - 1)

1) a(i-1)
Technical Definition: a(i-1) is the length of the perpendicular between the
joint axes. The joint axes is the axes around which revolution takes place
which are the Z(i-1) and Z(i) axes. These two axes can be viewed as lines
in space. The common perpendicular is the shortest line between the
two axis-lines and is perpendicular to both axis-lines.
a(i-1) cont...
Visual Approach - “A way to visualize the link parameter a(i-1) is to imagine
an expanding cylinder whose axis is the Z (i-1) axis - when the cylinder just
touches the joint axis i the radius of the cylinder is equal to a(i-1).” (Manipulator
Kinematics)

It’s Usually on the Diagram Approach - If the diagram already

specifies the various coordinate frames, then the common perpendicular
is usually the X(i-1) axis. So a(i-1) is just the displacement along the X(i-1) to
move from the (i-1) frame to the i frame.
If the link is prismatic, then a(i-1)
is a variable, not a parameter. Z(i - 1)

Yi Zi
Y(i -1)
Xi ai
a(i - 1 ) di
X(i -1)
i
( i - 1)
2) (i-1)
Technical Definition: Amount of rotation around the common
perpendicular
so that the joint axes are parallel.

i.e. How much you have to rotate around the X(i-1) axis so that the Z(i-1) is
pointing in the same direction as the Zi axis. Positive rotation follows
the right hand rule.
3) d(i-1)
Technical Definition: The displacement Z(i - 1)
along the Zi axis needed to align the a(i-1) Y(i -1)
Yi Zi
common perpendicular to the ai common Xi ai
a(i - 1 ) di
perpendicular. X(i -1)

In other words, displacement along ( i - i
the 1)

Zi to align the X(i-1) and Xi axes.

4) i
Amount of rotation around the Zi axis needed to align the X(i-1) axis with the
 The Denavit-Hartenberg Matrix

 cos θ i  sin θ 0 
i a (i  1)

 cos θ cos α  sin α  sin α d
 sin θ icos α (i  1) i (i  1) (i  1)
(i  1) i

 sin θ sin
i α (i  1)
cos θ i sin α (i  1) cos α (i  1)
cos α (i  1) d i 
 
 0 0 0 1 

Just like the Homogeneous Matrix, the Denavit-Hartenberg Matrix is

a transformation matrix from one coordinate frame to the next. Using
a series of D-H Matrix multiplications and the D-H Parameter table,
mfro
the final result is a transformation matrix some frame to your
frame. initial
Put the transformation here Zi
Z(i - 1)

Yi
1)
Y(i -1) Xi
di ai
Xa
(i -(i1)
-1)
Yi
 3 Revolute Joints Y2

Z0 Z1

X2
d2
X0 X1

Y0 Y1
Denavit-Hartenberg Link
a0 Parameter Table

i di i
 (i- 1) a (i- 1)
a1
0 0 0 0 0
Notice that the
table has two
1 0 a0 0 1
uses:
1) To describe
2 - 90 a1 d2 2
the robot with its
variables and
 Y2
i a (i- 1)
di i
 (i- 1)
Z0 Z1

0 0 0 0 0
X2
d2
X0 X1 1 0 a0 0 1

Y0 Y1
2 - 90 a1 d2 2

a0 a1

X2
V 

V Y 2
T  ( T)( 0 T)( 1
T)
X 0Y 0Z 0 1 2 0
V  T  Z 2

V 
 1  Note: T is the D-H matrix with (i-1) = 0 and i =1.
 
 i di i
 (i- 1) a (i- 1)  sin θ
 cos 0 0 0 0
θ
 cos θ 
0 0 0 0 0 T  sin θ
 0 0 0 0
0
 0 0 
1 0
 
1 0 a0 0 1  0 0
0 1
This is just a rotation around the Z0
2 - 90 a1 d2 2 axis

 cos θ 1  sin θ 1 0 a 0  cos θ 2

 sin θ 2 0 a1
cos θ  0 d 2

sin θ 1 1 0 0 
0 1 
T T
0 1
1 2
0 0 0 0   sin θ 2  cos θ 2
  
 0 0 1 0  0 0 0
0

This is a translation by
0 a1 and then1 d
2
This is a translation by a followed
0 by
a rotation around the Z1 axis followed by a rotation around the X2
and Z2 axis
0 1
T  (0 T) T)( T)
1 2
(
Inverse Kinematics
From Position to
Angles
 A Simple
Example

Revolute and
Finding :
Prismatic Joints
y
Combined θ  arctan( )

(x , y)
More x
Specifically: y arctan2() specifies that it’s in the
θ  arctan 2 ) first quadrant
(
Y x
S
1 Finding S:
X

S (x 2  y 2 )
 Inverse Kinematics of a Two Link
Manipulator
(x , Given: l1, l2 , x , y
y)
2 Find: 1 , 2
l2 Redundancy:
A unique solution to this problem
does not exist. Notice, that using
the “givens” two solutions are
l1
1 possible.
Sometimes no solution is possible.

(x , y)
 The Geometric
Solution l2 (x , Using the Law of Cosines:
θ2
y)
c 2  a 2  b 2  2 ab cos C
2 2
( x 2  y 2)  l  l  2 l l cos( 180  θ
)
1 2 1 2
2

cos( 180 2
θ 2
)
l1 cos( θ x
2 y l 2
l2)

1
cos(
2
θ 2) 2

 2 l1 l 2
θ1
2
x 2 y2 l l
2

θ2  arccos  
1 2

 2 l1 2 
Using the Law of Cosines: l
Redundant since 2 could be in the
sin B sin C
 first or fourth quadrant.
b c
sin θ sin(180
1
2 θ ) sin(
2 θ )
 
Redundancy caused since 2 has two
l2 x y
2 2
x y
2 2
possible values

θ 1  θ 1  l sin( θ ) y
α θ 1  arcsin  2 2 
  arctan 2 
y  x 2 y 2  x
α  arctan 2  
x
 The Algebraic Solution
l2 (x , y) 
θ2

c 1  cos θ 1
 cos( θ  θ )
c 12 2 1

l1 (1) x l c l c
1 1 2 12

(2) y  l s  l sin
1 1 2 12
θ1
(3) θ θ 1
θ 2

2 2 2 2
(1)  (2)  x  y 
c 1 (c 1  2 )   l s  l (sin
 l 2
c
2
 l
2
(c )
2 2
1
1
2
2
2
12 )
2
 2 l l s (sin )
 22l l l
1 l 1
2
2ll 2 1 c2 (c )  s (sin
12

1 2 1 12
1 1 22 1 12 1 12 )
2 2
 l1  l2  2 l1 2 c 2 Only
 Note :
l Unknown
2 2 2 2 cos( a  b )  (cos a )(cos b )  (sin a )(sin
y l l  b)
x  
θ  arccos   sin( a  b )  (cos a )(sin b )  (cos b )
1 2
2
 2 l1 2  (1sin a )
l  
 xl c l c Note :
1 1 2 12

l c l c 1 c 2  l 2 s1 s 2
cos( a  b )  (cos a )(cos b )  (sin a )(sin
1 1 2 b)
 
c ( l1  l c 2 )  s1 ( l 2 s 2 ) sin( a  b )  (cos a )(sin
1 2
b ) (cos b )(sin a)
 

y  l1 s 1  l 2 sin 12
We know what 2 is from the
l s s 1c 2  l 2 s 2c 1 previous slide. We need to solve
l
1
c
1
( l12 s
2
)s
2 1
(l l
1 2
c 2
) for 1 . Now we have two
equations and two unknowns (sin
c1 
x  s1 ( l 2 s 2 ) 1 and cos 1 )
( l1  l 2 c 2 )

x s (l s ) Substituting for c1 and

y ( l 2 s 2 )  s 1 ( l1  l c )
1 2 2
2 2 simplifying many times
( l1  l 2 c 2 )


1
x l s s (l 2
l 2  2l l c ) Notice this is the law of
( l1  l 2
c 2
)
2 2 1 1 2
 1 2 2
cosines and can be replaced
by x2+ y2

y ( l1  l 2 c 2 )  x l 2 s 2 
 y ( l1  l 2 c 2 )  x l 2 s 2 
s1  θ 1  arcsin
2 2  2 2 
x y  x y 