An Introduction to

Robot Kinematics
special description and
orientations
Lecture-2
By
Elias D.
 We are interested in two kinematics topics

✓Forward Kinematics (angles to position)

What you are given : The length of each link
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 given: The length of each link
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 
a 
Geometric Representation:  y
b x 
A • B = A B cosθ A b 
θ  y
Matrix Representation: B

a x  b x 
A • B =   •   = a xb x + a y b y
a y  b y 

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

a b   e f  (ae + bg ) (af + bh)

c d    g  =
h  (ce + dg ) (cf + dh)
  

Matrix Addition:
a b   e f   (a + e) (b + f )
c d  +  g  =
h  (c + g ) (d + h)
  
 Rotation Matrices in 3D – Ok, lets return from
homogenous repn
cosθ − sinθ 0
R z =  sinθ 0
Rotation around the Z-Axis
cosθ
 0 0 1

 cosθ 0 sinθ 
R y =  0 1 0  Rotation around the Y-Axis

− sinθ 0 cosθ

1 0 0 
R z = 0 cosθ − sinθ Rotation around the X-Axis

0 sinθ cosθ 

 Finding the Homogeneous Matrix
EX.

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

Z
W X   WI  WN 
 Y  Point relative to the  J  Point relative to the  O  Point relative to the
W  X-Y-Z frame  W  I-J-K frame W  N-O-A frame
 WZ  WK  W A 
     

 W   Pi   n i  WI   n i oi ai Pi  WN 
I
oi ai  W  N
 J   
 J    
a j  WO 

 W  = n j oj aj Pj  WO 
 W  =  Pj  +  n j oj
WK  Pk  nk ok ak  W A 
WK  nk ok ak Pk  W A 
      
 1   0 0 0 1   1 
 WN 
Y  O
W 
W A 
 
T
P
X

Z
WX  i x jx k x Tx   WI 
W  Tx  i x
X
jx k x   W I
 Y   
 Y      Ty   WJ 
jy k y   W J  W  = i y jy k y
W  = Ty  + i y  W Z  i z jz k z Tz  WK 
 W Z  Tz  i z jz k z  Wk      
   1   0 0 0 1   1 

WX  i x jx k x Tx   ni oi ai Pi  WN 
W   Y   
Ty   n j Pj  WO 
I

 
Substituting for  W J  W  = i y jy k y oj aj
WK 
 
 W Z  i z jz k z Tz  nk ok ak Pk  WA 
     
 1   0 0 0 1  0 0 0 1   1 
 W X  W N  i x jx k x Tx   ni oi ai Pi 
 Y  O i
 W  = H W  jy k y Ty   n j oj aj Pj 
WZ  W A  H= y
i z jz k z Tz  nk ok ak Pk 
      
 1   1  0 0 0 1  0 0 0 1
Product of the two matrices

Notice that H can also be written as:

1 0 0 Tx  i x jx k x 0 1 0 0 Pi   ni oi ai 0
0 1 0 Ty  i y jy k y 0 0 1 0 Pj   n j oj aj 0
H=
0 0 1 Tz  i z jz k z 0 0 0 1 Pk  nk ok ak 0
    
0 0 0 1  0 0 0 1 0 0 0 1  0 0 0 1

H = (Translation relative to the XYZ frame) * (Rotation relative to the XYZ frame)
* (Translation relative to the IJK frame) * (Rotation relative to the IJK frame)
 The Homogeneous Matrix is a concatenation of numerous
translations and rotations

WN 
Y  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. Here is an example…
Email:eliasdamite6@gmail.com
 An Introduction to
Robot Kinematics
special description and
orientations
Lecture-3
By
Elias D.
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 the
end of the robotic arm?

Solution:
1. Geometric Approach
This might be the easiest solution for the simple situation. 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.
Y3

θ3

Y2 2 X3 3
θ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.
Rotating by θ2 will put you in the X2Y2 frame.
θ1
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 the
coordinates of the yellow point relative the X0Y0 frame.
Slight variation on the last solution:
Make the yellow dot the origin of a new coordinate X4Y4 frame
Y3

Y4
θ3

Y2 2 X3 3
θ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 will
Z 0 equal the last column of the H matrix.
   
 
1 1
More on Forward Kinematics…

Denavit - Hartenberg
Parameters
 Denavit-Hartenberg Notation

Z(i - 1)

Y(i -1) Yi Zi

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 PARAMETERS/VARIABLES: θ, a , d, α
 The Parameters

You can
Z(i - 1) align the
two axis
Y(i -1) Yi Zi just using
Xi ai the 4
a(i - 1 ) di parameters
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.
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
along the Zi axis needed to align the a(i-1)
common perpendicular to the ai common
perpendicular.

In other words, displacement along the

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 Xi
axis.
 The Denavit-Hartenberg Matrix

 cosθi − sinθi 0 a(i −1) 

sinθ cosα cosθ cosα − sinα − sinα d 
 i (i −1) i (i −1) (i −1) (i −1) i 
sinθi sinα (i −1) cosθi sinα (i −1) cosα (i −1) cosα (i −1)di 
 
 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, the final result is a
transformation matrix from some frame to your initial frame.

Put the transformation here

 3 Revolute Joints Y2

Z0 Z1

X2
d2
X0 X1

Y0 Y1
Denavit-Hartenberg Link
a0 a1 Parameter Table

i (i-1) a(i-1) di i
Notice that the table has two uses:
1) To describe the robot with its 0 0 0 0 0
variables and parameters.
2) To describe some state of the 1 0 a0 0 1
robot by having a numerical values
for the variables. 2 -90 a1 d2 2
 Y2
i (i-1) a(i-1) di i
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

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

cosθ1 − sinθ1 0 a0   cosθ2 − sinθ2 0 a1 

sinθ cosθ1 0 0   0 0 1 d 2 
1T =
0  1
2T =
1 
 0 0 0 0 − sinθ2 − cosθ2 0 0
   
 0 0 0 1  0 0 0 1
This is a translation by a1 and then d2
This is a translation by a0 followed by a
followed by a rotation around the X2 and
rotation around the Z1 axis
Z2 axis

T =( 0T)( 01T)( 12T)

Exersise-1
Exersise-2
Exersise-3
Exersise-4
Email:eliasdamite6@gmail.com
 An Introduction to
Robot Kinematics
special description and
orientations
Lecture-4
By
Elias D.
Inverse Kinematics
From Position to Angles
 A Simple Example

Revolute and
Finding θ:
Prismatic Joints
y
Combined θ = arctan( )
x
More Specifically:
(x , y)
y arctan2() specifies that it’s in the
θ = arctan 2( ) first quadrant
x
Y

S
θ1 Finding S:
X

S = (x 2 + y2 )
Inverse Kinematics of a Two Link Manipulator

(x , y) Given: l1, l2 , x , y

θ2 Find: θ 1, θ 2
l2 Redundancy:
A unique solution to this problem
does not exist. Notice, that using the
“givens” two solutions are possible.
θ1
l1
Sometimes no solution is possible.

(x , y)
The Geometric Solution
(x , y) Using the Law of Cosines:
l2
θ2
c 2 = a 2 + b 2 − 2ab cos C
( x 2 + y 2 ) = l1 + l2 − 2l1l2 cos(180 − θ 2 )
2 2

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

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

c1 = cosθ1
c1+ 2 = cos(θ 2 + θ1 )
l1 (1) x = l1 c1 + l2 c1+ 2
(2) y = l1 s1 + l2 sin 1+ 2
θ1
(3) θ = θ1 + θ 2

(1) 2 + (2) 2 = x 2 + y 2 =
( 2 2
) (
= l1 c1 + l2 (c1+ 2 ) 2 + 2l1l2 c1 (c1+ 2 ) + l1 s1 + l2 (sin 1+ 2 ) 2 + 2l1l2 s1 (sin 1+ 2 )
2 2 2 2
)
= l1 + l2 + 2l1l2 (c1 (c1+ 2 ) + s1 (sin 1+ 2 ) )
2 2

= l1 + l2 + 2l1l2 c 2
2 2 Only Unknown Note :
 x 2 + y 2 − l12 − l2 2  cos(a−+b) = (cos a)(cos b)−+ (sin a)(sin b)
 θ 2 = arccos 
 sin( a−+b) = (cos a)(sin b)+− (cos b)(sin a)
 2 l1l 2 
 x = l1 c1 + l2 c1+ 2 Note :
= l1 c1 + l2 c1c 2 − l2 s1s2 cos(a−+b) = (cos a)(cos b)−+ (sin a)(sin b)
= c1 (l1 + l2 c 2 ) − s1 (l2 s2 ) sin( a−+b) = (cos a)(sin b)+− (cos b)(sin a)

y = l1 s1 + l2 sin 1+ 2 We know what 2 is from the previous

= l1 s1 + l2 s1c 2 + l2 s 2c1 slide. We need to solve for 1 . Now
= c1 (l2 s 2 ) + s1 (l1 + l2 c 2 ) we have two equations and two
unknowns (sin 1 and cos 1 )
x + s1 (l2 s2 )
c1 =
(l1 + l 2 c 2 )
x + s1 (l 2 s2 ) Substituting for c1 and simplifying
y= (l 2 s 2 ) + s1 (l1 + l2 c 2 ) many times
(l1 + l 2 c 2 )

=
1
(l1 + l2 c 2 )
(
x l 2 s2 + s1 (l12 + l 2 2 + 2l1l 2 c 2 )) Notice this is the law of cosines
and can be replaced by x2+ y2

y(l1 + l 2 c 2 ) − x l2 s2  y(l1 + l2 c 2 ) − x l2 s2 
s1 = θ1 = arcsin 

x 2 + y2  x 2 + y2 
Exersise-1
Exersise-2
Exersise-3
Exersise-4
Email:eliasdamite6@gmail.com