Professional Documents
Culture Documents
An Introduction to
Robot Kinematics
2
3
4
1
There are two more
joints on the end
effector (the gripper)
The PUMA 560 has SIX revolute joints
A revolute joint has ONE degree of freedom ( 1 DOF) that is
defined by its angle
Carnegie Mellon
Revolute Joint
1 DOF ( Variable - )
Prismatic Joint
1 DOF (linear) (Variables - d)
Spherical Joint
3 DOF ( Variables - 1, 2, 3)
Carnegie Mellon
What you can find: The angles of each joint needed to obtain
that position
Carnegie Mellon
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
Carnegie Mellon
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
Matrix Addition:
a b e f a e b f
c d g
h c g d h
Carnegie Mellon
Basic Transformations
Moving Between Coordinate Frames
(VN,VO)
VO
P
N
X VN
Px
V X V N Px
Notation: V XY
Y V NO
O P
V V 0
Carnegie Mellon
Writing V XY in terms of V NO
Y O
VO
P
N
X VN
PX V N
V XY
O P V NO
V
Carnegie Mellon
O
Translation along the X-Axis and Y-Axis
VO
N
VN
Px
X P XY
PY
PX V N
V XY P V NO O
Y
P V
Carnegie Mellon
V N V cosθ V NO cosθ V NO n
NO
V NO O NO NO NO
V V sinθ V cos(90 θ) V o
Carnegie Mellon
Y
Rotation (around the Z-Axis)
Z X
Y
VY
X
VX
V X V N
V XY Y V NO O
V V
Carnegie Mellon
x
Y Unit vector along X-Axis
V
Can be considered with respect to
the XY coordinates or NO coordinates
V
V XY V NO
VY
X
VX
V X V XY cosα V NO cosα V NO x
(Substituting for VNO using the N and O
V (V n V o) x
X N O
components of the vector)
V X V N (x n ) V O (x o)
V N (cosθ) V O (cos(θ 90))
V N (cosθ) V O (sinθ)
Carnegie Mellon
Similarly….
V Y V NO sinα V NO cos(90 α) V NO y
V Y (V N n V O o) y
V Y V N (y n) V O (y o)
V N (cos(90 θ)) V O (cosθ)
V N (sinθ) V O (cosθ)
So….
V V (cosθ) V (sinθ)
X N O V X
V XY Y
V Y V N (sinθ) V O (cosθ) V
Written in Matrix Form
(VN,VO)
Y0
VNO
VXY
X1
P
Translation along P followed by rotation by
X0
HOMOGENEOUS REPRESENTATION
Putting it all into a Matrix
V X cosθ sinθ Px V N
V Y sinθ cosθ Py V O Simplifying into a matrix form
1 0 0 1 1
cosθ sinθ Px
H sinθ cosθ Py Homogenous Matrix for a Translation in
XY plane, followed by a Rotation around
0 0 1 the z-axis
Carnegie Mellon
cosθ 0 sinθ
R y 0 1 0 Rotation around the Y-Axis
1 0 0
R z 0 cosθ sinθ Rotation around the X-Axis
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.
Carnegie Mellon
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 O
oy ay Py V 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.
Carnegie Mellon
Y W N
O
W
W A
T
P
X
Z
W X WI W N
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
W Z W K W A
W Pi n i
WI ni oi ai Pi W N
I
oi ai W N
J
J
a j W O
W n j oj aj Pj W O
W Pj n j oj
W K Pk n k W K n k Pk W A
ok a k W A
ok ak
1 0 0 0 1 1
Carnegie Mellon
W N
Y O
W
W A
T
P
X
Z
W X i x jx kx Tx W I
W Tx i x
X
jx k x W I
Y
Y Ty W J
k y W J W i y jy ky
W Ty i y jy
W Z i z jz kz Tz W K
W Z Tz i z jz k z W k
1 0 0 0 1 1
W X i x jx kx Tx n i oi ai Pi W N
W Y
Ty n j Pj W O
I
Substituting for W J W i y jy ky oj aj
W K W Z i z jz kz Tz n k ok ak Pk W A
1 0 0 0 1 0 0 0 1 1
Carnegie Mellon
W X W N i x jx kx Tx n i oi ai Pi
Y O i
W H W jy ky Ty n j oj aj Pj
WZ W A H y
i z jz kz Tz n k ok ak Pk
1 1 0 0 0 1 0 0 0 1
Product of the two matrices
1 0 0 Tx i x jx kx 0 1 0 0 Pi n i oi ai 0
0 1 0 Ty i y jy ky 0 0 1 0 Pj n j oj aj 0
H
0 0 1 Tz i z jz kz 0 0 0 1 Pk n k 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)
Carnegie Mellon
W N
Y O
W
W A
T
P
X
Z
One more variation on finding H:
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…
Carnegie Mellon
Forward Kinematics
Carnegie Mellon
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.
Carnegie Mellon
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
Y4
3
Y2 2 X3 3
2 X2
X4
Denavit-Hartenberg Notation
Z(i - 1)
Y(i -1) Yi Zi
Xi ai
a(i - 1 ) di
X(i -1)
i
( i - 1)
THE PARAMETERS/VARIABLES: , a , d,
Carnegie Mellon
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.
Carnegie Mellon
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.
2) (i-1)
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)
common perpendicular to the ai common Y(i -1) Yi Z
i
perpendicular. Xi ai
a(i - 1 ) di
X(i -1)
In other words, displacement along the i
Zi to align the X(i-1) and Xi axes. ( i -
1)
4) i
Amount of rotation around the Zi axis needed to align the X(i-1) axis with the Xi
axis.
Carnegie Mellon
sinθ i sinα (i 1) cosθ i sinα (i 1) cosα (i 1) cosα (i 1)d i
0 0 0 1
Z(i -
Put the transformation here 1) Y(i - Y Z
1) i i X a
a(i - d
X(i - i i
1) i
( i 1)
i
- 1)
Carnegie Mellon
Z0,1 Z2
X3
d2
X0,1 X2
-Y2
-Y0,1
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 1 0 0 0 1
variables and parameters.
2) To describe some state of the 2 0 a0 0 2
robot by having a numerical values
for the variables.
3 -90 a1 d2 3
Carnegie Mellon
Y3
i (i-1) a(i-1) di i
Z0, Z2
1
1 0 0 0 1
X3
d2
X0, X2 2 0 a0 0 2
Y0, 1 Y2
1 3 -90 a1 d2 3
a0 a1
V X 2
Y2
V T ( 0T)( 01T)(12T)
V X 0 Y0 Z 0
T Z
V 2
1 Note: T is the D-H matrix with (i-1) = 0 and i = 1.
Carnegie Mellon
0 0 1 0
2 0 a0 0 2
0 0 0 1
This is just a rotation around the Z0 axis
3 -90 a1 d2 3
T ( 0T)( 01T)(12T)
Carnegie Mellon
Next Example
Carnegie Mellon
Puma
Carnegie Mellon
Inverse Kinematics
From Position to Angles
Carnegie Mellon
A Simple Example
Revolute and
Finding :
Prismatic Joints
Combined θ arctan( y, x)
More Specifically:
(x , y)
arctan2() specifies that it’s in the
θ arctan 2( y, x) first quadrant
S
1 Finding S:
X
S (x 2 y 2 )
Carnegie Mellon
(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)
Carnegie Mellon
The Geometric Solution
Using the Law of Cosines:
(x , y)
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 2 y2 x 2 y2 values
θ1 α θ1 l sin(θ )
θ1 arctan 2 y, x arcsin 2 2
y x 2 y2
α arctan 2
x
Carnegie Mellon
The Algebraic Solution
(x , y)
l2 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
(1) 2 (2) 2 x 2 y 2
2 2
l1 c1 l2 (c1 2 ) 2 2l1l2 c1 (c1 2 ) l1 s1 l2 (sin1 2 ) 2 2l1l2 s1 (sin1 2 )
2 2 2 2
l1 l2 2l1l2 c1 (c1 2 ) s1 (sin1 2 )
2 2
l1 l2 2l1l2 c 2
2 2 Only Unknown Note :
x 2 y 2 l12 l2 2 cos(ab) (cos a)(cos b) (sin a)(sin b)
θ 2 arccos
sin(ab) (cos a)(sin b) (cos b)(sin a)
2 l l
1 2
Carnegie Mellon
x l1 c1 l2 c1 2 Note :
l1 c1 l2 c1c 2 l2 s1s2 cos(ab) (cos a)(cos b) (sin a)(sin b)
c1 (l1 l2 c 2 ) s1 (l2 s2 ) sin(ab) (cos a)(sin b) (cos b)(sin a)
1 arctan 2(s1 , c1 )
Carnegie Mellon
x l1 c1 l2 c1 2 Note :
l1 c1 l2 c1c 2 l2 s1s2 cos(ab) (cos a)(cos b) (sin a)(sin b)
c1 (l1 l2 c 2 ) s1 (l2 s2 ) sin(ab) (cos a)(sin b) (cos b)(sin a)
1
(l1 l2 c 2 )
Notice this is the law of cosines
x l2 s2 s1 (l1 l2 2l1l2 c 2 ) and can be replaced by x2+ y2
2 2