CS I400/B659:
Intelligent Robotics
Transformations and Matrix Algebra
Agenda
• Principles, Ch. 3.5-8
Rigid Objects
Biological systems, virtual
characters
Articulated Robot
q2
q1
• Robot: usually a rigid
articulated structure
• Geometric CAD models,
relative to reference
frames
• A configuration specifies
the placement of those
frames
Rigid Transformation in 2D
workspace
Frame T0
robot
ty
reference direction
q
reference point
tx
• Robot R0R2 given in reference frame T0
• Located at configuration q = (tx,ty,q) with q 
[0,2p)
Rigid Transformation in 2D
workspace
Frame T0
robot
P
reference direction
P
ty
q
reference point
tx
• Robot R0R2 given in reference frame T0
• Located at configuration q = (tx,ty,q) with q  [0,2p)
• Point P on the robot (e.g., a camera) has coordinates
𝑝0 in frame T0.
• What are the coordinates of P in the workspace?
Rigid Transformation in 2D
• Robot at configuration q = (tx,ty,q) with q  [0,2p)
• Point P on the robot (e.g., a camera) has coordinates
𝑝0 in frame T0.
• What are the coordinates of P in the workspace?
• Think of 2 steps: 1) rotating about the origin point by
angle q, then 2) translating the reference point to (tx,ty)
Rotations in 2D
-sin q
cos q
q
cos q
sin q
cos 𝜃
−sin 𝜃
• X axis of T0 gets coords 𝑥 =
, Y axis gets 𝑦 =
sin 𝜃
cos 𝜃
Rotations in 2D
-sin q
cos q
cos q
q
sin q
• X axis of T0 gets coords 𝑥 =
cos 𝜃
−sin 𝜃
, Y axis gets 𝑦 =
sin 𝜃
cos 𝜃
px
py
𝑝𝑦 ⋅ 𝑦
𝑝𝑥 ⋅ 𝑥
𝑝𝑥
• 𝑝 gets rotated to coords 𝑝𝑥 𝑥 + 𝑝𝑦 𝑦
𝑦
Rotations in 2D
-sin q
cos q
cos q
q
sin q
• X axis of T0 gets coords 𝑥 =
cos 𝜃
−sin 𝜃
, Y axis gets 𝑦 =
sin 𝜃
cos 𝜃
pxcos q -pysin q
px
py
𝑝𝑦 ⋅ 𝑦
𝑝𝑥 ⋅ 𝑥
𝑝𝑥
• 𝑝 gets rotated to coords 𝑝𝑥 𝑥 + 𝑝𝑦 𝑦
𝑦
pxsin q +
pycos q
Dot product
• For any P=(px,py) rotated by any q, we have the new coordinates
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
• We can express each element as a dot product:
𝑝𝑥
cos 𝜃
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃 =
⋅
− sin 𝜃 𝑝𝑦
𝑝𝑥
sin 𝜃
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃 =
⋅
cos 𝜃 𝑝𝑦
• Definition: 𝑐 = 𝑥 ⋅ 𝑦 = x1 y1 + x2 y2
• In 3D, 𝑐 = 𝑥 ⋅ 𝑦 = x1 y1 + x2 y2 + 𝑥3 𝑦3
• Key properties:
• Symmetric
• 0 only if 𝑥 and 𝑦 are perpendicular (orthogonal)
•
𝑥 = 𝑥⋅𝑥
Properties of the dot product
•
•
•
•
In 2D: 𝑥 ⋅ 𝑦 = x1 y1 + x2 y2
In 3D: 𝑥 ⋅ 𝑦 = x1 y1 + x2 y2 + 𝑥3 𝑦3
In n-D: 𝑥 ⋅ 𝑦 = 𝑛𝑖=1 𝑥𝑖 𝑦𝑖
Key properties:
• Symmetric
• 0 only if 𝑥 and 𝑦 are perpendicular (orthogonal)
•
𝑥 = 𝑥⋅𝑥
Properties of the dot product
•
•
•
•
In 2D: 𝑥 ⋅ 𝑦 = x1 y1 + x2 y2
In 3D: 𝑥 ⋅ 𝑦 = x1 y1 + x2 y2 + 𝑥3 𝑦3
In n-D: 𝑥 ⋅ 𝑦 = 𝑛𝑖=1 𝑥𝑖 𝑦𝑖
Key properties:
• Symmetric
• 0 only if 𝑥 and 𝑦 are perpendicular (orthogonal)
•
𝑥 = 𝑥⋅𝑥
𝑥
𝑣
𝑥⋅𝑣
If 𝑣 is a unit vector ( 𝑣 = 1)
then 𝑥 ⋅ 𝑣 is the length of the
projection of 𝑥 onto 𝑣.
Properties of the dot product
•
•
•
•
In 2D: 𝑥 ⋅ 𝑦 = x1 y1 + x2 y2
In 3D: 𝑥 ⋅ 𝑦 = x1 y1 + x2 y2 + 𝑥3 𝑦3
In n-D: 𝑥 ⋅ 𝑦 = 𝑛𝑖=1 𝑥𝑖 𝑦𝑖
Key properties:
• Symmetric
• 0 only if 𝑥 and 𝑦 are perpendicular (orthogonal)
•
𝑥 = 𝑥⋅𝑥
𝑥
If 𝑥 and 𝑦 are unit vectors with
inner angle 𝜃 then 𝑥 ⋅ 𝑦=cos 𝜃
𝑦
𝜃
cos 𝜃
Matrix-vector multiplication
• For any P=(px,py) rotated by any q, we have the new
coordinates
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
• We can express this as a matrix-vector product:
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
cos 𝜃 − sin 𝜃 𝑝𝑥
=
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
sin 𝜃 cos 𝜃 𝑝𝑦
Matrix-vector multiplication
• For any P=(px,py) rotated by any q, we have the new
coordinates
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
• We can express this as a matrix-vector product:
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
cos 𝜃 − sin 𝜃 𝑝𝑥
=
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
sin 𝜃 cos 𝜃 𝑝𝑦
• Or, 𝑦 = 𝐴𝑥 for A a 2x2 table of numbers
• Each entry of 𝒚 is the dot product between the
corresponding row of A and 𝒙
Matrix-vector multiplication
• For any P=(px,py) rotated by any q, we have the new
coordinates
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
• We can express this as a matrix-vector product:
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
cos 𝜃 − sin 𝜃 𝑝𝑥
=
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
sin 𝜃 cos 𝜃 𝑝𝑦
• Or, 𝑦 = 𝐴𝑥 for A a 2x2 table of numbers
• Each entry of 𝑦 is the dot product between the corresponding
row of A and 𝑥
Matrix-vector multiplication
• For any P=(px,py) rotated by any q, we have the new
coordinates
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
• We can express this as a matrix-vector product:
𝑝𝑥 cos 𝜃 − 𝑝𝑦 sin 𝜃
cos 𝜃 − sin 𝜃 𝑝𝑥
=
𝑝𝑥 sin 𝜃 + 𝑝𝑦 cos 𝜃
sin 𝜃 cos 𝜃 𝑝𝑦
• Or, 𝑦 = 𝐴𝑥 for A a 2x2 table of numbers
• Each entry of 𝑦 is the dot product between the corresponding
row of A and 𝑥
Matrix-vector product
examples
•
1 0 5.1
?
=
0 1 −7.3
?
•
3 0
2
?
=
1 −1 −7
?
3
• 1
0
0 0 2
?
−1 0 −7 = ?
0 5 −3
?
3
• 1
5
0 −1 2
?
−1 0 −7 = ?
0
5 −3
?
General equations
• 𝑦 = 𝐴𝑥
𝑎1,1 ⋯ 𝑎1,𝑛 𝑥1
𝑦1
⋱
⋮
⋮
• ⋮ = ⋮
𝑎𝑚,1 ⋯ 𝑎𝑚,𝑛 𝑥𝑛
𝑦𝑚
• A has dimensions m x n, 𝑦 has m entries, 𝑥 has n entries
• 𝑦𝑖 = 𝑛𝑗=1 𝑎𝑖,𝑗 𝑥𝑗 for each i=1,…,m
Matrix-vector product
examples
•
1 0
0 2
• 𝑎
4
0
−3 = ?
1
10
𝑐
𝑏 𝑑 =?
𝑐
•
𝑑 𝑎
𝑏 =?
Multiple rotations
cos 𝜃 − sin 𝜃
sin 𝜃 cos 𝜃
• We know that the new coordinates of a point 𝑥 rotated by 𝜃1
is given by 𝑦 = 𝑅(𝜃1 )𝑥
• What if we rotate again by 𝜃2 ? What are the new coordinates
𝑧?
• Define the 2D rotation matrix 𝑅 𝜃 =
Multiple rotations
cos 𝜃 − sin 𝜃
sin 𝜃 cos 𝜃
• We know that the new coordinates of a point 𝑥 rotated by 𝜃1
is given by 𝑦 = 𝑅(𝜃1 )𝑥
• What if we rotate again by 𝜃2 ? What are the new coordinates
𝑧?
• 𝑧 = 𝑅(𝜃2 )𝑦
• Define the 2D rotation matrix 𝑅 𝜃 =
Multiple rotations
cos 𝜃 − sin 𝜃
sin 𝜃 cos 𝜃
We know that the new coordinates of a point 𝑥 rotated by 𝜃1
is given by 𝑦 = 𝑅(𝜃1 )𝑥
What if we rotate again by 𝜃2 ? What are the new coordinates
𝑧?
𝑧 = 𝑅(𝜃2 )𝑦
𝑧 = 𝑅(𝜃2 )(𝑅 𝜃1 𝑥)
• Define the 2D rotation matrix 𝑅 𝜃 =
•
•
•
•
Multiple rotations
cos 𝜃 − sin 𝜃
sin 𝜃 cos 𝜃
We know that the new coordinates of a point 𝑥 rotated by 𝜃1
is given by 𝑦 = 𝑅(𝜃1 )𝑥
What if we rotate again by 𝜃2 ? What are the new coordinates
𝑧?
𝑧 = 𝑅(𝜃2 )𝑦
𝑧 = 𝑅(𝜃2 )(𝑅 𝜃1 𝑥)
• Define the 2D rotation matrix 𝑅 𝜃 =
•
•
•
•
Is it possible to define matrix-matrix multiplication so that
𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥?
Matrix-matrix multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
? ?
𝑎
•
=
? ?
𝑐
𝑏 𝑒
𝑑 𝑔
𝑓
ℎ
Matrix-matrix multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
Entry (1,1)
Row 1
? ?
𝑎
•
=
? ?
𝑐
Column 1
𝑏 𝑒
𝑑 𝑔
𝑓
ℎ
Matrix-matrix multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
Entry (1,2)
𝑎𝑒 + 𝑏ℎ
•
?
Row 1
𝑎
?
=
𝑐
?
Column 2
𝑏 𝑒
𝑑 𝑔
𝑓
ℎ
Matrix-matrix multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
Column 1
𝑎𝑒 + 𝑏ℎ
•
?
Entry (2,1)
𝑎𝑓 + 𝑏ℎ
𝑎
=
𝑐
?
𝑏 𝑒
𝑑 𝑔
Row 2
𝑓
ℎ
Matrix-matrix multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
Column 2
𝑎𝑒 + 𝑏ℎ
•
𝑐𝑒 + 𝑑ℎ
𝑎𝑓 + 𝑏ℎ
𝑎
=
𝑐
?
Entry (2,2)
𝑏 𝑒
𝑑 𝑔
Row 2
𝑓
ℎ
Matrix-matrix multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
𝑎𝑒 + 𝑏𝑔
•
𝑐𝑒 + 𝑑𝑔
𝑎𝑓 + 𝑏ℎ
𝑎
=
𝑐𝑓 + 𝑑ℎ
𝑐
𝑏 𝑒
𝑑 𝑔
𝑓
ℎ
Matrix-matrix multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
𝑎𝑒 + 𝑏𝑔
•
𝑐𝑒 + 𝑑𝑔
𝑎𝑓 + 𝑏ℎ
𝑎
=
𝑐𝑓 + 𝑑ℎ
𝑐
𝑎𝑒 + 𝑏𝑔
• Verify that
𝑐𝑒 + 𝑑𝑔
𝑏 𝑒
𝑑 𝑔
𝑓
ℎ
𝑎𝑓 + 𝑏ℎ 𝑥
𝑎
⋅ 𝑦 =
𝑐𝑓 + 𝑑ℎ
𝑐
𝑏
⋅
𝑑
𝑒
𝑔
𝑓
⋅
ℎ
Rotation matrix-matrix
multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
𝑎𝑒 + 𝑏𝑔
•
𝑐𝑒 + 𝑑𝑔
𝑎𝑓 + 𝑏ℎ
𝑎
=
𝑐𝑓 + 𝑑ℎ
𝑐
cos 𝜃2
𝑎 𝑏
•
=
sin 𝜃2
𝑐 𝑑
𝑒 𝑓
cos 𝜃1
•
=
sin 𝜃1
𝑔 ℎ
−sin 𝜃2
cos 𝜃2
−sin 𝜃1
cos 𝜃1
𝑏 𝑒
𝑑 𝑔
𝑓
ℎ
Rotation matrix-matrix
multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
•
𝑎𝑒 + 𝑏𝑔
𝑐𝑒 + 𝑑𝑔
𝑎𝑓 + 𝑏ℎ
𝑎
=
𝑐𝑓 + 𝑑ℎ
𝑐
𝑏 𝑒
𝑑 𝑔
𝑓
ℎ
cos 𝜃2 −sin 𝜃2
𝑎 𝑏
=
sin 𝜃2 cos 𝜃2
𝑐 𝑑
𝑒 𝑓
cos 𝜃1 −sin 𝜃1
•
=
sin 𝜃1 cos 𝜃1
𝑔 ℎ
• 𝑎𝑒 + 𝑏𝑔 = 𝑐𝑓 + 𝑑ℎ = cos 𝜃2 cos 𝜃1 − sin 𝜃2 sin 𝜃1 = cos(𝜃1 + 𝜃2 )
• 𝑎𝑓 + 𝑏ℎ = −𝑐𝑒 + 𝑑𝑔 = − cos 𝜃2 sin 𝜃1 − sin 𝜃2 cos 𝜃1 =
− sin(𝜃1 + 𝜃2 )
•
Rotation matrix-matrix
multiplication
• 𝑧 = 𝑅 𝜃2 𝑅 𝜃1 𝑥 so 𝑅 𝜃2 𝑅 𝜃1 must be 2x2
•
•
•
•
•
•
𝑎𝑒 + 𝑏𝑔
𝑐𝑒 + 𝑑𝑔
𝑎𝑓 + 𝑏ℎ
𝑎
=
𝑐𝑓 + 𝑑ℎ
𝑐
𝑏 𝑒
𝑑 𝑔
𝑓
ℎ
cos 𝜃2 −sin 𝜃2
𝑎 𝑏
=
sin 𝜃2 cos 𝜃2
𝑐 𝑑
𝑒 𝑓
cos 𝜃1 −sin 𝜃1
=
sin 𝜃1 cos 𝜃1
𝑔 ℎ
𝑎𝑒 + 𝑏𝑔 = 𝑐𝑓 + 𝑑ℎ = cos 𝜃2 cos 𝜃1 − sin 𝜃2 sin 𝜃1 = cos(𝜃1 + 𝜃2 )
𝑎𝑓 + 𝑏ℎ = −𝑐𝑒 + 𝑑𝑔 = − cos 𝜃2 sin 𝜃1 − sin 𝜃2 cos 𝜃1 =
− sin(𝜃1 + 𝜃2 )
So, 𝑹 𝜽𝟐 𝑹 𝜽𝟏 = 𝑹 𝜽𝟏 + 𝜽𝟐
General definition
• If A and B are m x p and p x n matrices, respectively, then the
matrix-matrix product is given by the m x n matrix C with
entries
𝑝
𝐶𝑖,𝑗 =
𝐴𝑖,𝑘 𝐵𝑘,𝑗
𝑘=1
Other Fun Facts
• An nxn identity matrix 𝐼 has 1’s on its diagonals and 0s
everywhere else
• 𝐼𝑥 = 𝑥 for all vectors 𝑥
• 𝐼𝐴 = 𝐴 for all nxm matrices 𝐴
• 𝐵𝐼 = 𝐵 for all mxn matrices 𝐵
• If A and B are square matrices such that 𝐴𝐵 = 𝐵𝐴 = 𝐼, then B
is called the inverse of A (and A is the inverse of B)
• Not all matrices are invertible
• The transpose of a matrix mxn matrix 𝐴 is the nxm matrix
formed swapping its rows and columns. It is denoted 𝐴𝑇 .
• i.e., 𝐴𝑇
• 𝐴𝐵
𝑇
𝑖,𝑗
= 𝐴𝑗,𝑖
= 𝐵𝑇 𝐴𝑇
Consequence: rotation inverse
• Since 𝑅 𝜃2 𝑅 𝜃1 = 𝑅 𝜃1 + 𝜃2 …
• 𝑅 𝜃 𝑅 −𝜃 = 𝑅 𝜃 − 𝜃 = 𝑅 0 = 𝐼 =
matrix)
1 0
(the identity
0 1
• But 𝑅 −𝜃 = 𝑅 𝜃 𝑇
• …so a rotation matrix’s inverse is its transpose.
Rigid Transformation in 2D
• q = (tx,ty,q) with q  [0,2p)
• Robot R0R2 given in reference frame T0
• What’s the new robot Rq?
{Tq(x,y) | (x,y)  R0}
• Define rigid transformation Tq(x,y) : R2 R2
Tq(x,y) =
cos θ -sin θ
sin θ cos θ
2D rotation matrix
x
y
+
tx
ty
Affine translation
Note: transforming points vs
directional quantities
• Rigid transform q = (tx,ty,q)
• A point with coordinates (x,y) in T0 undergoes
rotation and affine translation
Tq(x,y) =
cos θ -sin θ
sin θ cos θ
x
y
+
tx
ty
• Directional quantities (e.g., velocity, force) are
not affected by the affine translation!
Rq(vx,vy) =
cos θ -sin θ
sin θ cos θ
vx
vy
Next Lecture
• Optional: A Mathematical Introduction to Robotic
Manipulation, Ch. 2.1-3
• http://www.cds.caltech.edu/~murray/mlswiki/?title=First_edition