Introduction to ROBOTICS

Kinematics of Robot Manipulator

The City College of New York 1

 Outline
• Review

• Robot Manipulators
– Robot Configuration
– Robot Specification
• Number of Axes, DOF
• Precision, Repeatability

• Kinematics
– Preliminary
• World frame, joint frame, end-effector frame
• Rotation Matrix, composite rotation matrix
• Homogeneous Matrix
– Direct kinematics
• Denavit-Hartenberg Representation
• Examples
– Inverse kinematics

The City College of New York 2

 Review
• What is a robot?
– By general agreement a robot is:
• A programmable machine that imitates the actions or
appearance of an intelligent creature–usually a human.
– To qualify as a robot, a machine must be able to:
1) Sensing and perception: get information from its surroundings
2) Carry out different tasks: Locomotion or manipulation, do
something physical–such as move or manipulate objects
3) Re-programmable: can do different things
4) Function autonomously and/or interact with human beings
• Why use robots?
–Perform 4A tasks in 4D environments
4A: Automation, Augmentation, Assistance, Autonomous
4D: Dangerous, Dirty, Dull, Difficult

The City College of New York 3

 Manipulators
• Robot arms, industrial robot
– Rigid bodies (links) connected
by joints
– Joints: revolute or prismatic
– Drive: electric or hydraulic
– End-effector (tool) mounted
on a flange or plate secured
to the wrist joint of robot

The City College of New York 4

 Robotic Manipulators

• a robotic manipulator is a kinematic chain

– i.e. an assembly of pairs of rigid bodies that
can move respect to one another via a
mechanical constraint
• the rigid bodies are called links
• the mechanical constraints are called
joints

5 The City College of New York

 A150 Robotic Arm

link 3
link 2

6 The City College of New York

 Joints

• most manipulator joints are one of two types

1. revolute (or rotary)
– like a hinge
– allows relative rotation about a fixed axis between two links
• axis of rotation is the z axis by convention
2. prismatic (or linear)
– like a piston
– allows relative translation along a fixed axis between two links
• axis of translation is the z axis by convention
• our convention: joint i connects link i – 1 to link i
– when joint i is actuated, link i moves

7 The City College of New York

 Joint Variables

• revolute and prismatic joints are one degree of

freedom (DOF) joints; thus, they can be described
using a single numeric value called a joint variable
• qi : joint variable for joint i
1. revolute
– qi = qi : angle of rotation of link i relative to link i –
1
2. prismatic
– qi = di : displacement of link i relative to link i – 1

8 The City College of New York

 Revolute Joint Variable

• revolute
– qi = qi : angle of rotation of link i relative to link
link i
i–1

qi
link i – 1

9 The City College of New York

 Prismatic Joint Variable

• prismatic
– qi = di : displacement of link i relative to link i
–1
link i – 1 link i

di

10 The City College of New York

Common Manipulator Arrangments

• most industrial manipulators have six or

fewer joints
– the first three joints are the arm
– the remaining joints are the wrist
• it is common to describe such
manipulators using the joints of the arm
– R: revolute joint
– P: prismatic joint

11 The City College of New York

 Articulated Manipulator
• RRR (first three joints are all revolute)
• joint axes
– z0 : waist
z0
– z1 : shoulder (perpendicular to z0) z1 z2

– z2 : elbow (parallel to z1)

q2 q3
shoulder
forearm
elbow
q1

waist

12 The City College of New York

 Spherical Manipulator
• RRP
• Stanford armz 0
– z1
http://infolab.stanford.edu/pub/voy/museum/pictures/display/robots/IMG_2404ArmFrontPeeki
3d
ngOut.JPG
q2
shoulder z2

q1

waist

13 The City College of New York

 SCARA Manipulator
• RRP
• Selective Compliant
z
Articulatedz Robot for
1 2

Assemblyz 0
q 2

– http://www.robots.epson.com/products/g-series.htm

d3

q1

14 The City College of New York

 Manipulators
• Robot Configuration:

Cartesian: PPP Cylindrical: RPP Spherical: RRP

Hand coordinate:
SCARA: RRP n: normal vector; s: sliding vector;
Articulated: RRR (Selective Compliance a: approach vector, normal to the
Assembly Robot Arm)
tool mounting plate
The City College of New York 15
 Manipulators
• Motion Control Methods
– Point to point control
• a sequence of discrete points
• spot welding, pick-and-place, loading & unloading
– Continuous path control
• follow a prescribed path, controlled-path motion
• Spray painting, Arc welding, Gluing

The City College of New York 16

 Manipulators
• Robot Specifications
– Number of Axes
• Major axes, (1-3) => Position the wrist
• Minor axes, (4-6) => Orient the tool
• Redundant, (7-n) => reaching around
obstacles, avoiding undesirable
configuration
– Degree of Freedom (DOF)
– Workspace how accurately a specified point
can be reached
– Payload (load capacity)
– Precision v.s. Repeatability how accurately the same position
Which one is more important? can be reached if the motion is
repeated many times
The City College of New York 17
 What is Kinematics
• Forward kinematics
z

Given joint variables

q = (q1 , q2 , q3 , q4 , q5 , q6 ,  qn )
y

x
Y = ( x, y, z, O, A, T )

End-effector position and orientation, -Formula?

The City College of New York 18

 What is Kinematics
• Inverse kinematics
End effector position z
and orientation
( x, y, z, O, A, T )
y

q = (q1 , q2 , q3 , q4 , q5 , q6 ,  qn ) x

Joint variables -Formula?

The City College of New York 19

 Example 1

Forward kinematics y0
x1 = l cosq y1
x1
y1 = l sin q
l

Inverse kinematics q
x0
q = cos ( x1 / l )
−1

The City College of New York 20

 Example II
• given the joint variables and dimensions of the links what
is the position and orientation of the end effector?

a2

q2

a1

q1

21 The City College of New York

 Forward Kinematics
• choose the base coordinate frame of the robot
– we want (x, y) to be expressed in this frame
(x, y) ?

a2

q2
y0

a1

q1
x0

22 The City College of New York

 Forward Kinematics
• notice that link 1 moves in a circle centered on the base
frame origin
(x, y) ?

a2

q2
y0

a1

q1 ( a1 cos q1 , a1 sin q1 )
x0

23 The City College of New York

 Forward Kinematics
• choose a coordinate frame with origin located on joint 2
with the same orientation as the base frame
(x, y) ?

y1
a2

q2
y0
q1
a1 x1

q1 ( a1 cos q1 , a1 sin q1 )
x0

24 The City College of New York

 Forward Kinematics
• notice that link 2 moves in a circle centered on frame 1

(x, y) ?

y1
a2
( a2 cos (q1 + q2),
q2 a2 sin (q1 + q2) )
y0
q1
a1 x1

q1
( a1 cos q1 , a1 sin q1 )
x0

25 The City College of New York

 Forward Kinematics
• because the base frame and frame 1 have the same orientation, we
can sum the coordinates to find the position of the end effector in the
base frame (a1 cos q1 + a2 cos (q1 + q2),
a1 sin q1 + a2 sin (q1 + q2) )

y1
a2
( a2 cos (q1 + q2),
q2 a2 sin (q1 + q2) )
y0
q1
a1 x1

q1
( a1 cos q1 , a1 sin q1 )
x0

26 The City College of New York

 Forward Kinematics
• we also want the orientation of frame 2 with respect to
the base frame y2 x2
– x2 and y2 expressed in terms
of x0 and y0
a2

q2
y0
q1
a1

q1
x0

27 The City College of New York

 Forward Kinematics
x2 = (cos (q1 + q2),
y2
sin (q1 + q2) ) x2

y2 = (-sin (q1 + q2),

cos (q1 + q2) )
a2

q2
y0
q1
a1

q1
x0

28 The City College of New York

 Inverse Kinematics
• given the position (and possibly y2 x2
the orientation) of the end
(x, y)
effector, and the dimensions
of the links, what are the joint a
2
variables?
q2 ?
y0

a1

q1 ?
x0

29 The City College of New York

 Inverse Kinematics
• harder than forward kinematics because there is often
more than one possible solution
(x, y)

a2

y0

a1

x0

30 The City College of New York

 Inverse Kinematics
law of2 cosines
b = a1 + a2 − 2a1a2 cos( − q 2 ) = x 2 + y 2
2 2

(x, y)

a2
b
q2 ?
y0

a1

x0

31 The City College of New York

 Inverse Kinematics
x 2 + y 2 − a12 − a22
− cos( − q 2 ) =
2a1a2
and we have the trigonometric identity

− cos( − q 2 ) = cos(q 2 )
therefore,

x 2 + y 2 − a12 − a22
cosq 2 = = C2
2a1a2

We could take the inverse cosine, but this gives only one of the two solutions.

32 The City College of New York

 Inverse Kinematics
Instead, use the two trigonometric identities:
sin q
sin q + cos q 2 = 1
2 2
tan q =
cosq

to obtain

 1 − C 2
q 2 = tan −1 2

C2
which yields both solutions for q2 . In many programming languages you would use th
four quadrant inverse tangent function atan2

c2 = (x*x + y*y – a1*a1 – a2*a2) / (2*a1*a2);

s2 = sqrt(1 – c2*c2);
theta21 = atan2(s2, c2);
theta22 = atan2(-s2, c2);

33 The City College of New York

 Inverse Kinematics
 y −1  a2 sin q 2 
q1 = tan   − tan 
−1

x  a1 + a2 cosq 2 

34 The City College of New York

 Preliminary
• Robot Reference Frames
– World frame
– Joint frame
z
– Tool frame y
z x
y T P

W
x
R

The City College of New York 35

 Points and Vectors

• point : a location in space

• vector : magnitude (length) and
p
direction
between two points
v
q

36 The City College of New York

 Coordinate Frames

• choosing a frame (a point and two

perpendicular vectors of unit length) allows
0
us to assign coordinates p

4
p = 
0
ŷ 0 q0 2.5
2
q = 
0

x̂0 1.5
0 o0  2
v = p −q =  
0 0 0

1 
37 The City College of New York
 Coordinate Frames

• the coordinates change depending on the

choice of frame
p1

1 o1
ŷ1 − 0.5
p =
1

q1  4 
0.5
x̂1 q = 
1

2
− 1
v = p −q =  
1 1 1

2
38 The City College of New York
 Preliminary
• Coordinate Transformation
– Reference coordinate frame
OXYZ z
– Body-attached frame O’uvw P
Point represented in OXYZ:
y
Pxyz = [ p x , p y , p z ]
T
w
 v
Pxyz = p x i x + p y jy + p z k z
Point represented in O’uvw:
x
 O, O’
u
Puvw = pu i u + pv jv + pw k w
Two frames coincide ==> pu = p x pv = p y pw = p z
The City College of New York 39
 Preliminary
Properties: Dot Product
Let x and y be arbitrary vectors in R and q be
3

the angle from x to y , then

x  y = x y cos q
Properties of orthonormal coordinate frame
• Mutually perpendicular • Unit vectors
  
i  j =0 | i |= 1
  
i k = 0 | j |= 1
  
k j =0 | k |= 1
The City College of New York 40
 Preliminary
• Coordinate Transformation
– Rotation only z
P

Pxyz = p x i x + p y jy + p z k z
 y
Puvw = pu i u + pv jv + pw k w w v

Pxyz = RPuvw u
x

How to relate the coordinate in these two frames?

The City College of New York 41

 Preliminary
• Basic Rotation
– p x , p y, and pz represent the projections of P
onto OX, OY, OZ axes, respectively

– Since P = pu i u + pv jv + pw k w

px = i x  P = i x  i u pu + i x  jv pv + i x  k w pw
p y = jy  P = jy  i u pu + jy  jv pv + jy  k w pw

pz = k z  P = k z  i u pu + k z  jv pv + k z  k w pw

The City College of New York 42

 Preliminary
• Basic Rotation Matrix
 px   i x  i u i x  jv i x  k w   pu 
 p  =  j i jy  jv  
jy  k w   pv  
 y  y u
 p z  k z  i u k z  jv k z  k w   pw 
– Rotation about x-axis with q
z
w
1 0 0  P v

Rot ( x,q ) = 0 Cq 
− Sq 
q
0 Sq Cq  y
u
x
The City College of New York 43
 Preliminary
• Is it True?
– Rotation about x axis withq
 p x  1 0 0   pu 
 p  = 0 cosq   
− sin q   pv 
 y  z
 p z  0 sin q cosq   pw  w
P v
p x = pu
q
p y = pv cosq − pw sin q
y
p z = pv sin q + pw cosq u
x
The City College of New York 44
 Basic Rotation Matrices
– Rotation about x-axis with q
1 0 0 
Rot ( x,q ) = 0 Cq − Sq 
0 Sq Cq 
– Rotation about y-axis with q
 Cq 0 Sq 
Rot ( y,q ) =  0 1 0 
− Sq 0 Cq 

– Rotation about z-axis with q

Cq − Sq 0
Pxyz = RPuvw Rot ( z,q ) =  Sq Cq 0
 0 0 1
The City College of New York 45
 Preliminary
• Basic Rotation Matrix
 i x  i u i x  jv i x  k w 
R =  jy  i u jy  jv jy  k w  Pxyz = RPuvw
k z  i u k z  jv k z  k w 
– Obtain the coordinate of Puvw from the coordinate
of Pxyz Dot products are commutative!

 pu   i u  i x i u  jy i u  k z   px 
 p  =  j i jv  j y

jv  k z   p y  Puvw = QPxyz
 v  v x
 pw  k w  i x k w  jy k w  k z   p z 
−1
Q=R =R T

QR = R T R = R −1 R = I 3 <== 3X3 identity matrix

The City College of New York 46

 Properties of Rotation Matrices
• RT = R-1
• the columns of R are mutually orthogonal
• each column of R is a unit vector
• det R = 1 (the determinant is equal to 1)

47 The City College of New York 2/13/2023

 Example
• A point auvw = (4,3,2) is attached to a rotating frame,
the frame rotates 60 degree about the OZ axis of
the reference frame. Find the coordinates of the
point relative to the reference frame after the
rotation.
a xyz = Rot ( z ,60)auvw
 0.5 − 0.866 0 4 − 0.598
= 0.866 0.5 0 3 =  4.964 
 0 0 1 2  2 

The City College of New York 48

 Example
• A point a xyz = (4,3,2) is the coordinate w.r.t. the
reference coordinate system, find the
corresponding point auvw w.r.t. the rotated
OU-V-W coordinate system if it has been
rotated 60 degree about OZ axis.

auvw = Rot ( z,60)T a xyz

 0.5 0.866 0 4  4.598 
= − 0.866 0.5 0 3 = − 1.964
 0 0 1 2  2 

The City College of New York 49

 Composite Rotation Matrix
• A sequence of finite rotations
– matrix multiplications do not commute
– rules:
• if rotating coordinate O-U-V-W is rotating about
principal axis of OXYZ frame, then Pre-multiply
the previous (resultant) rotation matrix with an
appropriate basic rotation matrix
• if rotating coordinate OUVW is rotating about its
own principal axes, then post-multiply the
previous (resultant) rotation matrix with an
appropriate basic rotation matrix

The City College of New York 50

 Example
• Find the rotation matrix for the following
operations:
R = Rot ( y,  ) I 3 Rot ( w,q ) Rot (u,  )
Rotation  about OY axis
 C 0 S  Cq − Sq 0 1 0 0 
Rotation q about OW axis =  0 1 0   Sq Cq 0 0 C − S 
Rotation  about OU axis - S 0 C   0 0 1 0 S C 
 CCq SS − CSqC CSqS + SC 
=  Sq CqC − CqS 

Answer ... − SCq SSqC + CS CC − SSqS 

Pre-multiply if rotate about the OXYZ axes

Post-multiply if rotate about the OUVW axes
The City College of New York 51
 Translation

ŷ 0 ŷ1

x̂0 x̂1
o0 o1
0 
1

3
o = 
0
1
0 
• suppose we are given o1 expressed in {0}
52 The City College of New York
 Translation 1

ŷ 0 ŷ1

x̂0 x̂1
o0 o1
0 
1

 3   0   3
d = o −o =  −  =  
0
1
0
1
0
0
 0  0  0 
• the location of {1} expressed in {0}
53 The City College of New York
 Translation 1
i
d
1. the translation vector j can be
interpreted as the location of frame {j}
expressed in frame {i}

54 The City College of New York

 Translation 2
a point expressed
in frame {1}

ŷ 0 ŷ1 1
p = 
1

1
x̂0 x̂1
o0 o1
0 
1

3 1 4

p =d + p = +  = 
0 0
1
1

0 1 1

• p1 expressed in {0}
55 The City College of New York
 Translation 2
i
d
2. the translation vector j can be
interpreted as a coordinate transformation
of a point from frame {j} to frame {i}

56 The City College of New York

 Translation 3

− 1 ŷ 0  2
p = 
0
q = 
0

1 1 
x̂0
o0
0

3 − 1 2
q =d + p = +  = 
0 0

0   1   1 
• q0 expressed in {0}
57 The City College of New York
 Translation 3
3. the translation vector d can be
interpreted as an operator that takes a
point and moves it to a new point in the
same frame

58 The City College of New York

 Coordinate Transformations
• position vector of P
in {B} is transformed
to position vector of P
in {A}

• description of {B} as
seen from an observer
in {A}

Rotation of {B} with respect to {A}

Translation of the origin of {B} with respect to origin of {A}

The City College of New York 59
 Coordinate Transformations
• Two Special Cases
rPA = ARB rPB + roA'
1. Translation only
– Axes of {B} and {A} are
parallel A
RB = 1

2. Rotation only
– Origins of {B} and {A}
are coincident
roA' = 0
The City College of New York 60
Homogeneous Representation
• Coordinate transformation from {B} to {A}
rP = RB rP + ro'
A A B A

 rPA   A RB roA'   rPB 

 =  
 1   013 1  1 
• Homogeneous transformation matrix
 RB r   R33 P31 
A A Rotation
matrix
TB = 
A
=
o'

 01 3 1  0 1  Position
vector
Scaling
The City College of New York 61
Homogeneous Transformation
• Special cases
1. Translation
 I 33 roA' 
A
TB =  
 01 3 1

2. Rotation

 A
RB 031 
A
TB =  
 013 1 

The City College of New York 62

 Example
• Translation along Z-axis with h:
1 0 0 0  x  1 0 0 0  pu   pu 
0 1 0 0   y  0 1 0 0  pv   pv 
Trans( z , h) =   = =
0 0 1 h  z  0 0 1 
h pw    pw + h 
        
0 0 0 1  1  0 0 0 1  1   1 
z z P
P
w y
y
w v
v
x O, O’
u x
h
O, O’
u
The City College of New York 63
 Example
• Rotation about the X-axis by
1 0 0 0  x  1 0 0 0  pu 
 0 Cq 0  y   0 Cq
− Sq − Sq 0  pv 
Rot ( x, q ) =   =
0 Sq Cq 0  z  0 Sq Cq 0  p w 
      
0 0 0 1  1  0 0 0 1  1 
z
w
P v

y
u
x
The City College of New York 64
 Homogeneous Transformation
• Composite Homogeneous Transformation
Matrix
• Rules:
– Transformation (rotation/translation) w.r.t
(X,Y,Z) (OLD FRAME), using pre-
multiplication
– Transformation (rotation/translation) w.r.t
(U,V,W) (NEW FRAME), using post-
multiplication

The City College of New York 65

 Example
• Find the homogeneous transformation matrix
(T) for the following operations:
Rotation  about OX axis
Translation of a along OX axis
Translation of d along OZ axis
Rotation of q about OZ axis
T = Tz ,q Tz ,d Tx ,aTx , I 44
Answer : Cq − Sq 0 0 1 0 0 0  1 0 0 a  1 0 0 0
 Sq Cq 0 0 0 1 0 0  0 1 0 0  0 C − S 0
=
 0 0 1 0  0 0 1 d  0 0 1 0  0 S C 0
    
 0 0 0 1  0 0 0 1  0 0 0 1  0 0 0 1

The City College of New York 66

 Homogeneous Representation
• A frame in space (Geometric
Interpretation) P( px , p y , pz )
z
 R33 P31  a (z’)
F = 
s(y’)
 0 1  n (X’)
y
 nx sx ax px 
n sy ay p y 
F = y
 nz sz az pz  x
 
0 0 0 1
Principal axis n w.r.t. the reference coordinate system

The City College of New York 67

 Homogeneous Transformation
• Translation a
s
1 0 0 d x   nx s x ax px 
z n
0 1 0 d y  n y s y ay p y 
a
Fnew =  s
0 0 
1 d z  nz s z az pz 
    n
0 0 0 1 0 0 0 1
 nx sx ax px + d x  y
n s y a y p y + d y 
= y
 nz sz az pz + d z 
 
0 0 0 1 
Fnew = Trans (d x , d y , d z )  Fold
The City College of New York 68
Homogeneous Transformation
Composite Homogeneous Transformation Matrix
z1 z 2 y2

y1 x2
z0 0
A1 1
A2
y0
x1
x0
?
i −1 Transformation matrix for
Ai adjacent coordinate frames

0
A2 =0A11 A2 Chain product of successive
coordinate transformation matrices

The City College of New York 69

 Example
• For the figure shown below, find the 4x4 homogeneous transformation
matrices i −1 Ai and 0 Ai for i=1, 2, 3, 4, 5
 n s a p  − 1 0 0 0 
c x
n s a
x x x
  0 0 −1 e + c 
 py  0 A =  
z3 F= y y y
1
 0 −1 0 a − d 
b y3 x  nz s z a z p z   
z5
3
d    0 0 0 1 
0 0 0 1 
x5 0 − 1 0 b 
y5 z4 0 0 − 1 a − d 
A2 =  
e 1

x4 y4 z2 1 0 0 0 
a
x2  
 0 0 0 1 
z1 x1 y2
z0 0 1 0 −b 
y1 − 1 0 0 e + c 
x0
y0
Can you find the answer by observation
0
A2 = 
0 0 1 0 
based on the geometric interpretation of  
homogeneous transformation matrix? 0 0 0 1 
The City College of New York 70
 Orientation Representation
 R33 P31 
F = 
 0 1 
• Rotation matrix representation needs 9
elements to completely describe the
orientation of a rotating rigid body.
• Any easy way?

Euler Angles Representation

The City College of New York 71

 Orientation Representation
• Euler Angles Representation (  , q,  )
– Many different types
– Description of Euler angle representations

Euler Angle I Euler Angle II Roll-Pitch-Yaw

Sequence  about OZ axis  about OZ axis  about OX axis
of q about OU axis q about OV axis q about OY axis
Rotations  about OW axis  about OW axis  about OZ axis

The City College of New York 72

 Euler Angle I, Animated

w'= z

w'"= w"
 v'"
v"

v'
y

u'"
q
u' =u"
x

The City College of New York 73

 Orientation Representation
• Euler Angle I
 cos − sin  0 1 0 0 
   
Rz =  sin  cos 0 , Ru 'q =  0 cosq − sin q ,
 1   0 sin q cosq 
 0 0 
 cos − sin  0
 
Rw'' =  sin  cos 0
 0 1 
 0

The City College of New York 74

 Euler Angle I
Resultant eulerian rotation matrix:

R = Rz Ru 'q Rw''

 cos cos − cos sin  
 sin  sin q 
 − sin  sin  cosq − sin  cos cosq 
 
 
 sin  cos − sin  sin  − cos sin q 
 + cos sin  cosq + cos cos cosq 
 
 
 sin  sin q cos sin q cosq 
 
The City College of New York 75
 Euler Angle II, Animated

w'= z

w"'= w" 

 v"'
q v' =v"
y
u"'
u"
Note the opposite u'
(clockwise) sense of the x
third rotation, .
The City College of New York 76
 Orientation Representation
• Matrix with Euler Angle II
 − sin  sin  − sin  cos 
 cos sin q 
 + cos cos cosq − sin  cos cosq 
 
 
 cos sin  cos cos sin  sin q 
 + sin  cos cosq − sin  cos cosq 
 
 cosq 
 − cos sin q sin  sin q
 
Quiz: How to get this matrix ?
The City College of New York 77
 Orientation Representation
• Description of Roll Pitch Yaw
Z

 Y
q
X

Quiz: How to get rotation matrix ?

The City College of New York 78

 Inverse Transformation
• the inverse of a transformation undoes the
original transformation
– if
 R d
T = 
0 0 0 1 

– then
−1  R T
− R T
d
T = 
0 0 0 1 
79 The City College of New York
 Thank you!
Homework 1 is posted on the web.

Next class: kinematics II

z
y
z
y x
z z
x y
y x

x
The City College of New York 80