Introduction to Robotics

Dr. Hassan SHRAIM

 Rotation Matrices in 3D
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 x  0 cosθ  sinθ  Rotation around the X-Axis

0 sinθ cosθ 

 Homogeneous Matrices in 3D
H is a 4x4 matrix that can describe a translation, rotation, or both in one
matrix O
1 0 0 Px 
Y
0 1 0 Py 
N H
P
0 0 1 Pz 
X A  
Translation without rotation 0 0 0 1
Z

Y
n x ox ax 0
n oy ay 0
H y
O
N
n z oz az 0
 
X 0 0 0 1
Rotation part:
Z Rotation without translation Could be rotation around z-axis,
A x-axis, y-axis or a combination of
the three.
 Homogeneous Continued….
V N 
 O The (n,o,a) position of a point relative to the
V
V XY
 H A  current coordinate frame you are in.
V 
 
 1 

n x ox ax Px  V N 
n  
oy ay Py  V O  V X  n x V N  o x V O  a x V A  Px
V XY  y
n z oz az Pz  V A 
  
0 0 0 1   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.
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

(l3, 0). Multiplying H by that position vector will give you the
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

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
Denavit-Hartenberg Convention
 Denavit-Hartenberg Convention
• Number the joints from 1 to n starting with the base and ending with the
end-effector.
• Establish the base coordinate system. Establish a right-handed
orthonormal coordinate system (X0,Y0,Z0at ) the supporting base with Z 0
axis lying along the axis of motion of joint 1.
• Establish joint axis. Align the Zi with the axis of motion (rotary or sliding)
of joint i+1.
• Establish the origin of the ith coordinate system. Locate the origin of the
ith coordinate at the intersection of the Zi & Zi-1 or at the intersection of
common normal between the Zi & Zi-1 axes and the Zi axis.
• Establish Xi axis. Establish Xi (Z
i1Zi)/Z
i1 Zi or along the common

normal between the Zi-1 & Zi axes when they are parallel.
• Establish Yi axis. Assign Y i  (Zi X i)/Z i X i
to complete the right-
handed coordinate system.
• Find the link and joint parameters
 Example I
• 3 Revolute Joints

Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

Link 1 Link 2 d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1
 Link Coordinate Frames
• Assign Link Coordinate Frames:
– To describe the geometry of robot motion, we assign a Cartesian
coordinate frame (Oi, Xi,Yi,Zi) to each link, as follows:
• establish a right-handed orthonormal coordinate frame O0 at the
supporting base with Z0 lying along joint 1 motion axis.
• the Zi axis is directed along the axis of motion of joint (i + 1), that is,
link (i + 1) rotates about or translates along Zi;
Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

Link 1 Link 2 d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1
 Link Coordinate Frames
– Locate the origin of the ith coordinate at the intersection of
the Zi & Zi-1 or at the intersection of common normal between
the Zi & Zi-1 axes and the Zi axis.
– the Xi axis lies along the common normal from the Zi-1 axis to
the Zi axis X i (Z
i1Zi)
/Zi
1Z,i(if Zi-1 is parallel to Zi, then Xi is
specified arbitrarily, subject only to Xi being perpendicular to
Zi); Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1
 Link Coordinate Frames
– Assign Yi (ZiXi)/Z
itoXcomplete
i the right-handed
coordinate system.
• The hand coordinate frame is specified by the geometry ofO the
n
end-effector. Normally, establish Zn along the direction of Zn-1
axis and pointing away from the robot; establish Xn such that it
is normal to both Zn-1 and Zn axes. Assign Yn to complete the
right-handed coordinate system.
Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1
 Link and Joint Parameters
• Joint angle  i: the angle of rotation from the Xi-1 axis to the Xi
axis about the Zi axis. It is the joint variable if joint i is rotary.

• Joint distance d i : the distance from the origin of the (i-1)

coordinate system to the intersection of the Zi-1 axis and the Xi
axis along the Zi-1 axis. It is the joint variable if joint i is
prismatic.

• Link length a:i the distance from the intersection of the Zi-1 axis
and the Xi axis to the origin of the ith coordinate system along
the Xi axis.

• Link twist angle  :i the angle of rotation from the Zi-1 axis to the
Zi axis about the Xi axis.
 Example I Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

d2
Joint 1
O0 X0 O1 X1 O2 X2 D-H Link Parameter Table
Joint 2
Y2 Joint i i ai di i

a0 a1 1 0 a0 0 0

 i : rotation angle from Zi-1 to Zi about Xi 2 -90 a1 0 1

ai : distance from intersection of Zi-1 & Xi 3 0 0 d2 2

to origin of i coordinate along Xi
di : distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1
i : rotation angle from Xi-1 to Xi about Zi-1
 Example II: PUMA 260
1. Number the joints
2. Establish base frame
1 2 3. Establish joint axis Zi
Z1 4. Locate origin, (intersect.
O1 3 of Zi & Zi-1) OR (intersect
of common normal & Zi )
X1 Z2 Z6
Y1 5. Establish Xi,Yi
O2
Y3 Z Z4
O3 X 2 5  6 Y6 i
X(
Z Z
i)
/Z Z
5
i1 i1 i
Y2
O i
Y 
(i
Z X
i)
/Zi
X
Z0 6 i
Y
X3 4 Y5
t
O5 X5 X6
O4 Z
3

X4 4
PUMA 260
 Link Parameters
J i i ai d i
1 2 1 1 -90 0 13
Z1 2 2 0 8 0
O1 3 3 3 90 0 -l
X1 Z2 Z6 4 4 -90 0 8
Y1 O 5 5 90 0 0
Y3 2 Z Z4
O3 X 2 5  6 Y6 6 6 0 0 t
Y2
O
5  i : angle from Xi-1 to Xi
Z0 6
X3 4Y Y5 about Zi-1
O5
O4 Z
X5 X6  i : angle from Zi-1 to Zi
3 about Xi
X4 4 ai : distance from intersection
of Zi-1 & Xi to Oi along Xi
Joint distance d i : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1
 Transformation between i-1 and i
• Four successive elementary transformations are
required to relate the i-th coordinate frame to the
(i-1)-th coordinate frame:
– Rotate about the Z i-1 axis an angle of i to align the X i-1
axis with the X i axis.
– Translate along the Z i-1 axis a distance of di, to bring Xi-1
and Xi axes into coincidence.
– Translate along the Xi axis a distance of ai to bring the two
origins Oi-1 and Oi as well as the X axis into coincidence.
– Rotate about the Xi axis an angle of αi ( in the right-
handed sense), to bring the two coordinates into
coincidence.
 Transformation between i-1 and i
• D-H transformation matrix for adjacent coordinate
frames, i and i-1.
– The position and orientation of the i-th frame coordinate can
be expressed in the (i-1)th frame by the following
homogeneous transformation matrix:
Source coordinate

Ti
i1T(z
i1,d
i)R(z
i1,i)
T (x
i,a
i)R(x
i,i)

Ci C iS i SiS  i a

iC  i
Reference S C C  S C  a S 
Coordinate  i i i i i i i

0 S i C i d i

 
 0 0 0 1 
 Kinematic Equations
• Forward Kinematics q(q
1,q
2,q
n)
– Given joint variables
– End-effector position & orientation Y(x
,y ,
,z ,,
)
• Homogeneous matrix T0n
– specifies the location of the ith coordinate frame w.r.t. the
base coordinate system
– chain product of successive coordinate transformation
matrices of Ti i 1
Position
Tn
T
0T
T 12
01
n

n1 vector

Orientation Rn n
P 
n s a n
P
matrix 
0 0

0

0 1 0 0 0 1
 Kinematics Equations
• Other representations
– reference from, tool frame

ref
tool 0 n tool
T B T0H
ref n

– Yaw-Pitch-Roll representation for orientation

TRz,Ry,Rx,
C S 0 0 C 0 S 0 1 0 0 0
S C 0 0  0 1 0 0 0 C S 0
   
0 0 1 0S 0 C 0 0 S C 0
   
0 0 0 1 0 0 0 1 0 0 0 1
 Solving forward kinematics
• Forward kinematics • Transformation Matrix

 1  p x 
  p  nx sx ax px
 2   y  n
 3  p  sy ay py
T  y 
    
z
nz sz az pz 
 4     
 5    0 0 0 1
   
  6    
 Solving forward kinematics
• Yaw-Pitch-Roll representation for orientation

C CCS 
S
S 
CC
SC
S
Sp


 
 p
x

S CSSSCCSSCCS 
n  
0
   p
y
T

S CS CC  z
 
0 0 0 
1
nx sx ax px sin
1
(nz)
n s a p  1 a
T0n  y y y y cos

( z )
nz sz az pz  cos
  1 n
0 0 0 1  cos

( x )

cos
Problem? Solution is inconsistent and ill-conditioned!!
 atan2(y,x)
y

0

 
90 for
xand
y


 

90
  
180 
for 
xandy
atan
2
(y
,x
)  

 
180
90

for 
xand
y


90


0 
for 
xandy