Robotics 1

Direct kinematics
Prof. Alessandro De Luca

Robotics 1 1
 Kinematics of robot manipulators

n study of ...
geometric and timing aspects of robot motion,
without reference to the causes producing it

n robot seen as ...

an (open) kinematic chain of rigid bodies
interconnected by (revolute or prismatic) joints

Robotics 1 2
 Motivations
n functional aspects
n definition of robot workspace
n calibration
n operational aspects
task execution task definition and
(actuation by motors) performance

two different “spaces” related by kinematic (and dynamic) maps

n trajectory planning
n programming
n motion control

Robotics 1 3
 Kinematics
formulation and parameterizations

𝑟 = 𝑓(𝑞)
JOINT DIRECT TASK
space (Cartesian)
INVERSE space

𝑞 = (𝑞1, … , 𝑞𝑛) 𝑞 = 𝑓 -. (𝑟) 𝑟 = (𝑟1, … , 𝑟𝑚)

n choice of parameterization 𝑞
n unambiguous and minimal characterization of robot configuration
n n = # degrees of freedom (dof) = # robot joints (rotational or
translational)
n choice of parameterization 𝑟
n compact description of position and/or orientation (pose) variables
of interest to the required task
n usually, 𝑚 £ 𝑛 and 𝑚 6 (but none of these is strictly necessary)
Robotics 1 4
 Open kinematic chains

𝑞2 𝑞𝑛 𝑅𝐹𝐸
𝑞4
𝑞1 𝑞3 𝑟 = (𝑟1, … , 𝑟2 )
e.g., the relative angle e.g., it describes the
between a link and the pose of frame 𝑅𝐹𝐸
following one

n 𝑚=2 n 𝑚=5
n pointing in space n positioning and pointing in space
n positioning in the plane (like for spot welding)
n 𝑚=3 n 𝑚=6
n orientation in space n positioning and orientation in space
n positioning and orientation in the plane n positioning of two points in space
(e.g., end-effector and elbow)
Robotics 1 5
 Classification by kinematic type
first 3 dofs only

SCARA
(RRP)

cylindric
Cartesian or (RPP)
gantry
(PPP) articulated or
anthropomorphic
(RRR)
polar or
spherical
(RRP)

R = 1-dof rotational (revolute) joint

P = 1-dof translational (prismatic) joint
Robotics 1 6
 Direct kinematic map

n the structure of the direct kinematics function

depends on the chosen 𝑟

𝑟 = 𝑓7 (𝑞)

n methods for computing 𝑓7 (𝑞)

n geometric/by inspection
n systematic: assigning frames attached to the robot
links and using homogeneous transformation matrices

Robotics 1 7
 Direct kinematics of 2R planar robot
just using inspection…
𝑦 𝑞.
𝑃 𝜙
𝑝𝑦 • 𝑞= 𝑞 𝑛=2
D
𝑙2
𝑞2 𝑝E
𝑙1 𝑟 = 𝑝F 𝑚=3
𝑞1 𝜙
𝑝𝑥 𝑥

𝑝𝑥 = 𝑙1 cos𝑞1 + 𝑙2 cos(𝑞1 + 𝑞2)

𝑝𝑦 = 𝑙1 sin𝑞1 + 𝑙2 sin(𝑞1 + 𝑞2)
𝜙 = 𝑞1 + 𝑞2
for more general cases, we need a ‘method’!
Robotics 1 8
 Numbering links and joints

joint 2 joint 𝑖 + 1
link 𝑖 − 1 joint 𝑛
link 1
link 𝑛
joint 1 link 𝑖
joint 𝑖 − 1
link 0 joint 𝑖
(base) (end effector)

icon representation of joint types

for the manipulator skeleton

revolute prismatic
Robotics 1 9
 Spatial relation between joint axes

axis of joint 𝑖 axis of joint 𝑖 + 1

90° 𝜋
𝐴
𝛼M

90°

common normal
𝐵 (axis of link 𝑖 )

𝑎M = displacement 𝐴𝐵 between joint axes (always well defined)

with sign
𝛼M = twist angle between joint axes (pos/neg)!
— projected on a plane 𝜋 orthogonal to the link axis

Robotics 1 10
 Spatial relation between link axes
axis of joint 𝑖
link 𝑖 − 1

link 𝑖
𝐷
𝐶
axis of link 𝑖 − 1 axis of link 𝑖
𝜎
𝜃M

𝑑M = displacement 𝐶𝐷 (a variable if joint 𝑖 is prismatic)

with sign
𝜃M = angle between link axes (a variable if joint 𝑖 is revolute) (pos/neg)!
— projected on a plane 𝜎 orthogonal to the joint axis
Robotics 1 11
 Denavit-Hartenberg (DH) frames
joint axis joint axis joint axis
𝑖−1 𝑖 𝑖+1
link 𝑖 − 1
link 𝑖

link 𝑖 − 1 is moved
by joint 𝑖 − 1 frame 𝑅𝐹𝑖 is
attached to link 𝑖 𝛼M
𝑧M
𝑧M-. 𝑎M
𝑑M 𝑂M 𝑥M
𝑥M-.
𝜃M
𝑂M-. axis of link 𝑖
axis of joint 𝑖 as common normal to
around which link 𝑖 rotates joint axes 𝑖 and 𝑖 + 1
or along which link 𝑖 slides

Robotics 1 12
 Definition of DH parameters
joint axis joint axis joint axis
𝑖−1 𝑖 𝑖+1
link 𝑖
link 𝑖 − 1
𝛼M
𝐷 𝑧M
•
𝑧M-. 𝑎M
𝑑M 𝑂M
𝑥M-. 𝑥M
𝜃M
𝑂M-.

n unit vector 𝑧𝑖 along axis of joint 𝑖 + 1

n unit vector 𝑥𝑖 along the common normal to joint 𝑖 and 𝑖 + 1 axes (𝑖 ® 𝑖 + 1)
n 𝑎M = distance 𝐷𝑂M , + if oriented as 𝑥M , always constant (= ‘length’ of link 𝑖 )
n 𝑑M = distance 𝑂M-. 𝐷, + if oriented as 𝑧M-. , variable if joint 𝑖 is PRISMATIC
n 𝛼M = twist angle from 𝑧M-. to 𝑧𝑖 around 𝑥M , + if CCW, always constant
n 𝜃M = angle from 𝑥M-. to 𝑥𝑖 around 𝑧M-. , + if CCW, variable if joint 𝑖 is REVOLUTE
Robotics 1 13
 DH layout made simple
a popular 3-minute illustration...

video

https://www.youtube.com/watch?v=rA9tm0gTln8

n note: the author of this video uses 𝑟 in place of 𝑎 , and does not add subscripts!
Robotics 1 14
 Homogeneous transformation
between successive DH frames (from frame 𝑖 − 1 to frame 𝑖 )

§ roto-translation (screw motion) around and along 𝑧M-.

cos 𝜃M − sin 𝜃M 0 0 1 0 0 0 cos 𝜃M − sin 𝜃M 0 0
0 = sin 𝜃M cos 𝜃M 0 0
M-.
𝐴M X 𝑞M = sin 𝜃M cos 𝜃M 0 0 0
0
1
0
0
1 𝑑M
0 0 1 0 0 0 1 𝑑M
0 0 0 1 0 0 0 1 0 0 0 1
the product of these two matrices commutes!

rotational joint ⇒ 𝑞𝑖 = 𝜃M prismatic joint ⇒ 𝑞𝑖 = 𝑑M

§ roto-translation (screw motion) around and along 𝑥M

1 0 0 𝑎M
MX 0 cos 𝛼M − sin 𝛼M 0 always a
𝐴M =
0 sin 𝛼M cos 𝛼M 0 constant matrix
0 0 0 1

Robotics 1 15
 Denavit-Hartenberg matrix

J. Denavit and R.S. Hartenberg, “A kinematic notation for lower-pair mechanisms based on matrices,”
Trans. ASME J. Applied Mechanics, 23: 215–221, 1955

cos 𝜃M − cos 𝛼M sin 𝜃M sin 𝛼M sin 𝜃M 𝑎M cos 𝜃M

M-. M-. MX sin 𝜃M cos 𝛼M cos 𝜃M − sin 𝛼M cos 𝜃M 𝑎M sin 𝜃M
𝐴M 𝑞M = 𝐴M X 𝑞M 𝐴M =
0 sin 𝛼M cos 𝛼M 𝑑M
0 0 0 1

compact notation: 𝑐 = cos, 𝑠 = sin

super-compact notation (if feasible): 𝑐𝑖 = cos 𝑞M , 𝑠M = sin 𝑞M

Robotics 1 16
 Ambiguities in defining DH frames
n frame 0: origin and 𝑥0 axis are arbitrary
n frame 𝑛: 𝑧[ axis is not specified
n however, 𝑥[ must intersect and be chosen orthogonal to 𝑧[-.

n positive direction of 𝑧M-. (up/down on axis of joint 𝑖 ) is arbitrary

n choose one, and try to ‘avoid flipping over’ to the next one
n positive direction of 𝑥M (back/forth on axis of link 𝑖 ) is arbitrary
n if successive joint axes are incident, we often take 𝑥M = 𝑧M-. × 𝑧M

n when natural, follow the direction ‘from base to tip’

n if 𝑧M-. and 𝑧M are parallel (common normal not uniquely defined)
n 𝑂M chosen arbitrarily along 𝑧M , still trying to ‘zero out’ parameters

n if 𝑧M-. and 𝑧M are coincident, normal 𝑥M axis can be chosen at will

n this case occurs only if the two joints are of different kind (P/R or R/P)
n again, try using ‘simple values’ (e.g., 0 or ±𝜋/2) for constant angles
Robotics 1 17
 Direct kinematics of robot manipulators
𝑦𝐸 slide 𝑠
description ‘internal’ 𝑅𝐹𝐸
to the robot using • 𝑞 = (𝑞1, … , 𝑞𝑛)
• product of DH matrices 𝑧𝐸 approach 𝑎
b
𝐴. 𝑞. .𝐴D 𝑞D ⋯ [-.𝐴[ 𝑞[
𝑥𝐸 normal 𝑛
𝑧0
𝑅𝐹0
𝑅𝐹_ description ‘external’
𝑦0
to the robot using
_ _
𝑥0 𝑅 𝑝_a
• _ 𝑇a = a
0e 1
𝑛 𝑠 𝑎 𝑝
_ _ =
𝑇a = 𝑇b b𝐴. 𝑞. .
𝐴D 𝑞D ⋯ [-.𝐴[ 𝑞[ [
𝑇a 0e 1
• 𝑟 = (𝑟1, … , 𝑟2 )
𝑟 = 𝑓7 (𝑞)
alternative representations of the direct kinematics
Robotics 1 18
 Direct kinematics of 2R planar robot
using DH frame assignment…

𝑦b 𝑦D 𝑥D 𝑧b , 𝑧. , 𝑧D outgoing from plane

𝑃 𝜙
𝑝𝑦 • 𝑖 𝛼M 𝑎M 𝑑M 𝜃M
𝑎D = 𝑙 D
𝑦. 1 0 𝑙. 0 𝑞.
link 2 𝜃D = 𝑞D
2 0 𝑙D 0 𝑞D
𝑎. = 𝑙. 𝑥.
• joint 2
link 1 cos 𝑞. + 𝑞D
𝜃. = 𝑞.
joint 1
•
= cos 𝑞. 𝑝𝑥 𝑥b 𝑐.D −𝑠.D 0 𝑙. 𝑐. + 𝑙D 𝑐.D
link 0 b 𝑠.D 𝑐.D 0 𝑙. 𝑠. + 𝑙D 𝑠.D
𝑐𝜃. −𝑠𝜃. 0 𝑙. 𝑐𝜃. 𝐴D 𝑞 =
0 0 1 0
b 𝑠𝜃. 𝑐𝜃. 0 𝑙. 𝑠𝜃.
𝐴. 𝜃. = 0 0 0 1
0 0 1 0
𝑝E 0 𝑙. 𝑐. + 𝑙D 𝑐.D
0 0 0 1 𝑝 𝑝 0
𝑐𝜃D −𝑠𝜃D 0 𝑙D 𝑐𝜃D = F = b𝐴D 𝑞 = 𝑙. 𝑠. + 𝑙D 𝑠.D
1 0 0 0
. 𝑠𝜃D 𝑐𝜃D 0 𝑙D 𝑠𝜃D 1 1 1
𝐴D 𝜃D =
0 0 1 0 𝜙 = 𝑞. + 𝑞D (extracted from b𝑅D 𝑞 )
0 0 0 1
Robotics 1 19
 Direct kinematics of 2R planar robot
TCP location on the robot end effector

𝑦b 𝑦D 𝑥D
𝑃 𝑦a 𝑖 𝛼M 𝑎M 𝑑M 𝜃M
• • 1 0 𝑙. 0 𝑞.
𝑙D 𝑇𝐶𝑃 𝑧a
𝑦.
𝑞D approach 2 0 𝑙D 0 𝑞D
axis
𝑥. 𝑐.D −𝑠.D
𝑙. • 0 𝑙. 𝑐. + 𝑙D 𝑐.D
b 𝑠 𝑐.D 0 𝑙. 𝑠. + 𝑙D 𝑠.D
𝑞. 𝐴D 𝑞 = .D
• 0 0 1 0
𝑥b 0 0 0 1
Tool Center Point 𝑇𝐶𝑃 and associated end-effector frame 𝑅𝐹a
D b D
0 1 0 𝑇𝐶𝑃E 𝑇𝐶𝑃E (𝑞) 𝑇𝐶𝑃E 0
b
D 𝑇𝐶𝑃(𝑞) = b D 0
D
𝑇a = 0 0 −1 𝑇𝐶𝑃F 𝑇𝐶𝑃F (𝑞) = b𝐴D 𝑞 𝑇𝐶𝑃F = b𝑇a 𝑞
−1 0 0 0 1 0 0 0
0 0 0 1 1 1 1
= b𝐴D 𝑞 D
𝑇a
Robotics 1 20
 DH assignment for a SCARA robot

video

𝑞3

𝑞1 𝑞2
𝑞4

Sankyo SCARA 8438 Sankyo SCARA SR 8447

Robotics 1 21
 Step 1: joint axes

all parallel
(or coincident)

twist angles J3 prismatic

𝛼M = 0 or p ≡
J2 elbow
J4 revolute

J1 shoulder

Robotics 1 22
 Step 2: link axes

the vertical ‘heights’

of the link axes
are arbitrary
(for the time being)
𝑎D
𝑎. 𝑎f = 0

Robotics 1 23
 Step 3: frames

𝒛𝟏
𝒙𝟏 𝒛𝟐 = 𝒛𝟑
axes 𝒚𝒊 for 𝑖 > 0
are not shown 𝒙𝟐
(nor needed; they form
right-handed frames) 𝒙𝟑
𝒙𝟒
𝒛𝟎
𝒛𝟒 = 𝒂 axis
(approach)
𝒚𝟎
𝒙𝟎

Robotics 1 24
 Step 4: DH table of parameters

𝑖 𝛼M 𝑎M 𝑑M 𝜃M

𝒛𝟏
1 0 𝑎. 𝑑. 𝑞.
𝒙𝟏 𝒛𝟐 = 𝒛𝟑 2 0 𝑎D 0 𝑞D
𝑎1

𝑎2 𝒙𝟐 3 0 0 𝑞f 0
𝑞f
𝑑4 𝒙𝟑 4 𝜋 0 𝑑r 𝑞r
𝑑1
𝒛𝟎 𝒙𝟒
𝒛𝟒 note that
• 𝑑. and 𝑑r could be set = 0
𝒚𝟎 • 𝑑r < 0 (opposite to 𝒛f )
𝒙𝟎
• also, 𝑞f < 0 in this configuration

Robotics 1 25
 Step 5: DH transformation matrices

𝑐𝜃. −𝑠𝜃. 0 𝑎. 𝑐𝜃.

b 𝑠𝜃. 𝑐𝜃. 0 𝑎. 𝑠𝜃.
𝐴. 𝑞. =
0 0 1 𝑑.
0 0 0 1
𝑐𝜃D −𝑠𝜃D 0 𝑎D 𝑐𝜃D 1 0 0 0
𝑠𝜃D 𝑐𝜃D 0 𝑎D 𝑠𝜃D
.
𝐴D 𝑞D = D
𝐴f 𝑞f = 0 1 0 0
0 0 1 0 0 0 1 𝑑f
0 0 0 1 0 0 0 1
𝑐𝜃r 𝑠𝜃r 0 0
𝑞 = (𝑞1, 𝑞2, 𝑞3, 𝑞4) f 𝑠𝜃r −𝑐𝜃r 0 0
𝐴r 𝑞r =
0 0 −1 𝑑r
= (𝜃. , 𝜃D , 𝑑f , 𝜃r )
0 0 0 1

Robotics 1 26
 Step 6a: direct kinematics
_ M-.
homogeneous matrix 𝑇a as product of the 𝐴M (𝑞M )’s

𝑐.D −𝑠.D 0 𝑎. 𝑐. + 𝑎D 𝑐.D

b 𝑠.D 𝑐.D 0 𝑎. 𝑠. + 𝑎D 𝑠.D
𝐴D 𝑞. , 𝑞D =
0 0 1 𝑑.
0 0 0 1
𝑐.D −𝑠.D 0 𝑎. 𝑐. + 𝑎D 𝑐.D
b 𝑠.D 𝑐.D 0 𝑎. 𝑠. + 𝑎D 𝑠.D
𝐴f 𝑞. , 𝑞D , 𝑞f =
0 0 1 𝑑. + 𝑞f
0 0 0 1
𝑅(𝑞1, 𝑞2, 𝑞4) = [ 𝑛 𝑠 𝑎 ] 𝑝 = 𝑝(𝑞1, 𝑞2, 𝑞3)
𝑐.Dr 𝑠.Dr 0 𝑎. 𝑐. + 𝑎D 𝑐.D
_ b 𝑠.Dr −𝑐.Dr 0 𝑎. 𝑠. + 𝑎D 𝑠.D
𝑇a = 𝐴r 𝑞. , 𝑞D , 𝑞f , 𝑞r =
0 0 −1 𝑑. + 𝑞f + 𝑑r
( _ 𝑇b = r
𝑇a = 𝐼) 0 0 0 1
Robotics 1 27
 Step 6b: direct kinematics
as task vector 𝑟 ∈ ℝ2

𝑐.Dr 𝑠.Dr 0 𝑎. 𝑐. + 𝑎D 𝑐.D

b 𝑠.Dr −𝑐.Dr 0 𝑎. 𝑠. + 𝑎D 𝑠.D
𝐴r 𝑞. , 𝑞D , 𝑞f , 𝑞r =
0 0 −1 𝑑. + 𝑞f + 𝑑r
0 0 0 1
extract 𝛼y
from take 𝑝 ∈ ℝr
𝑅(𝑞1, 𝑞2, 𝑞4) as such from
𝑝E 𝑎. 𝑐. + 𝑎D 𝑐.D 𝑝(𝑞1, 𝑞2, 𝑞3)
𝑝F 𝑎. 𝑠. + 𝑎D 𝑠.D
𝑟= = 𝑓7 𝑞 = ∈ ℝr
𝑝y 𝑑. + 𝑞f + 𝑑r
𝛼y 𝑞. + 𝑞D + 𝑞r

Robotics 1 28
 Stanford manipulator
n 6-dof: 2R-1P-3R (spherical wrist)
n robot with shoulder offset
n ‘one possible’ DH assignment of
frames is shown
n determine the associated
n table of DH parameters
n homogeneous transformation
matrices
n direct kinematics
n write a program for computing
the direct kinematics
n numerically (Matlab), given a 𝒒
n symbolically (Mathematica,
Maple, Symbolic Manipulation
Toolbox of Matlab, …)
Robotics 1 29
 DH table for Stanford manipulator
n 6-dof: 2R-1P-3R (spherical wrist)

𝑖 𝛼M 𝑎M 𝑑M 𝜃M
1 −𝜋/2 0 𝑑1 > 0 𝑞1 = 0
2 𝜋/2 0 𝑑2 > 0 𝑞2 = 0
3 0 0 𝑞3 > 0 −𝜋/2
4 −𝜋/2 0 0 𝑞4 = 0
5 𝜋/2 0 0 𝑞5 = −𝜋/2
6 0 0 𝑑6 > 0 𝑞6 = 0
joint variables are in red, while their values
in the robot configuration shown are in blue
Robotics 1 30
 KUKA LWR 4+
n 7R (no offsets, spherical shoulder and spherical wrist)

available at
DIAG Robotics Lab

n determine
n frames and table
of DH parameters
n homogeneous
transformation
matrices
n direct kinematics
n 𝑑. and 𝑑{ can be
set = 0 or not
(as needed)
left right
side view (from the left) frontal view
Robotics 1 31
 KUKA KR5 Sixx R650
n 6R (offsets at shoulder and elbow, spherical wrist)

top view

n determine side view (from observer in V)

n frames and table of DH parameters
available at
n homogeneous transformation matrices
DIAG Robotics Lab
n direct kinematics
Robotics 1 32
 Appendix:
Modified DH convention
n a modified version introduced in J. Craig’s book “Introduction to Robotics”
(1986) and aligned for the indexing by Khalil and Kleinfinger (ICRA, 1986)
n has 𝑧M axis on joint 𝑖

n 𝑎M & 𝛼M = distance & twist angle from 𝑧M-. to 𝑧M , measured along & about 𝑥M-.

n 𝑑M & 𝜃M = distance & angle from 𝑥M-. to 𝑥M , measured along & about 𝑧M

n source of much confusion... if you are not aware of it (or don’t mention it!)
n convenient with link flexibility: a rigid frame at the base, another at the tip...
classical y2 x2
(or distal)
𝑐𝜃M −𝑐𝛼M 𝑠𝜃M 𝑠𝛼M 𝑠𝜃M 𝑎M 𝑐𝜃M planar 2R
M-. 𝑠𝜃M 𝑐𝛼M 𝑐𝜃M −𝑠𝛼M 𝑐𝜃M 𝑎M 𝑠𝜃M y1 example
𝐴M =
0 𝑠𝛼M 𝑐𝛼M 𝑑M y0 x1
0 0 0 1
modified
(or proximal) x0 y2 x2
𝑐𝜃M −𝑠𝜃M 0 𝑎M y1 y0
x1
M-. |}~ 𝑐𝛼M 𝑠𝜃M 𝑐𝛼M 𝑐𝜃M −𝑠𝛼M −𝑑M 𝑠𝛼M
𝐴M =
𝑠𝛼M 𝑠𝜃M 𝑠𝛼M 𝑐𝜃M 𝑐𝛼M 𝑑M 𝑐𝛼M x0
0 0 0 1 modified DH tends to place frames
Robotics 1 ‘at the base’ of each link 33