1. ECE 383/ME 442: Intro to Robotics and Automation
Linear Algebra for 3D Transformations 1

2. Agenda Readings Rigid transformations Principles, 3.5-6
Planning Algorithms, Ch. 3.2
Rigid transformations

3. Rigid Objects

4. Biological systems, virtual characters

5. Articulated Robot Robot: usually a rigid articulated structure
Geometric CAD models, relative to movable reference frames
A configuration specifies the placement of those frames q1 q2

6. Rigid Transformations
At the heart of robotics…
For planning and control
Collision avoidance
Deciding how to move in joint space to produce desired motion in workspace
For modeling physics
Effects of gravity on joints
Understanding how contact forces affect joint torques
Convert data gathered in one coordinate frame to another…
From distance readings to absolute positions
An “eye-in-hand” camera
Two 3D scans from two different poses

7. Coordinates, Points and Displacements
A point X in space can be represented for the purposes of calculations (not in the realm of pure thought) as a set of coordinates in a privileged reference frame TW, called the world frame.
The representation of a point P is an array of numbers, called its coordinates
The coordinates change depending on the choice of FW
When comparing or manipulating two points, their representations as vectors must use the reference frames!
The coordinates of a displacement (or direction) from point X to Y are the same regardless of the world frame origin
But they change depending on its orientation
Only the following operations make sense (in a common frame) :
P-P=D, P+D=P, D+D=D

8. Rigid Transformation in 2D
workspace
Frame T0
robot
reference direction q ty
reference point tx
Robot R0R2 given in reference frame T0
Located at configuration q = (tx,ty,q) with q  [0,2p) 8

9. Rigid Transformation in 2D
workspace
Frame T0
robot
reference direction P P q ty
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? 9

10. 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)

11. Rotations in 2D
-sin q
X axis of T0 gets coords 𝑥 = cos 𝜃 sin 𝜃 , Y axis gets 𝑦 = −sin 𝜃 cos 𝜃
cos q
cos q q
sin q

12. Rotations in 2D
-sin q
X axis of T0 gets coords 𝑥 = cos 𝜃 sin 𝜃 , Y axis gets 𝑦 = −sin 𝜃 cos 𝜃
𝑝 𝑥 𝑝 𝑦 gets rotated to coords 𝑝 𝑥 𝑥 + 𝑝 𝑦 𝑦
cos q
cos q q
sin q px py
𝑝 𝑥 ⋅ 𝑥
𝑝 𝑦 ⋅ 𝑦

13. Rotations in 2D
-sin q
X axis of T0 gets coords 𝑥 = cos 𝜃 sin 𝜃 , Y axis gets 𝑦 = −sin 𝜃 cos 𝜃
𝑝 𝑥 𝑝 𝑦 gets rotated to coords 𝑝 𝑥 𝑥 + 𝑝 𝑦 𝑦
cos q
cos q q
sin q
pxcos q -pysin q px
pxsin q + pycos q py
𝑝 𝑥 ⋅ 𝑥
𝑝 𝑦 ⋅ 𝑦

14. 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 𝜃 − 𝑝 𝑦 sin 𝜃 = cos 𝜃 − sin 𝜃 ⋅ 𝑝 𝑥 𝑝 𝑦
𝑝 𝑥 sin 𝜃 + 𝑝 𝑦 cos 𝜃 = sin 𝜃 cos 𝜃 ⋅ 𝑝 𝑥 𝑝 𝑦
Definition: 𝑐= 𝑥 ⋅ 𝑦 = x 1 y 1 + x 2 y 2
In 3D, 𝑐= 𝑥 ⋅ 𝑦 = x 1 y 1 + x 2 y 2 + 𝑥 3 𝑦 3
Key properties:
Symmetric
0 only if 𝑥 and 𝑦 are perpendicular (orthogonal)
𝑥 = 𝑥 ⋅ 𝑥

15. Properties of the dot product
In 2D: 𝑥 ⋅ 𝑦 = x 1 y 1 + x 2 y 2
In 3D: 𝑥 ⋅ 𝑦 = x 1 y 1 + x 2 y 2 + 𝑥 3 𝑦 3
In n-D: 𝑥 ⋅ 𝑦 = 𝑖=1 𝑛 𝑥 𝑖 𝑦 𝑖
Key properties:
Symmetric
0 only if 𝑥 and 𝑦 are perpendicular (orthogonal)
𝑥 = 𝑥 ⋅ 𝑥

16. Properties of the dot product
In 2D: 𝑥 ⋅ 𝑦 = x 1 y 1 + x 2 y 2
In 3D: 𝑥 ⋅ 𝑦 = x 1 y 1 + x 2 y 2 + 𝑥 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 𝑣 . 𝑣
𝑥 ⋅ 𝑣

17. Properties of the dot product
In 2D: 𝑥 ⋅ 𝑦 = x 1 y 1 + x 2 y 2
In 3D: 𝑥 ⋅ 𝑦 = x 1 y 1 + x 2 y 2 + 𝑥 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 𝜃

18. 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 𝜃 𝑝 𝑥 sin 𝜃 + 𝑝 𝑦 cos 𝜃 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 𝑝 𝑥 𝑝 𝑦

19. 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 𝜃 𝑝 𝑥 sin 𝜃 + 𝑝 𝑦 cos 𝜃 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 𝑝 𝑥 𝑝 𝑦
Or, 𝑦 =𝐴 𝑥 for A a 2x2 matrix
Each entry of 𝒚 is the dot product between the corresponding row of A and 𝒙

20. 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 𝜃 𝑝 𝑥 sin 𝜃 + 𝑝 𝑦 cos 𝜃 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 𝑝 𝑥 𝑝 𝑦
Or, 𝑦 =𝐴 𝑥 for A a 2x2 matrix
Each entry of 𝑦 is the dot product between the corresponding row of A and 𝑥

21. 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 𝜃 𝑝 𝑥 sin 𝜃 + 𝑝 𝑦 cos 𝜃 = cos 𝜃 − sin 𝜃 sin 𝜃 cos 𝜃 𝑝 𝑥 𝑝 𝑦
Or, 𝑦 =𝐴 𝑥 for A a 3x3 matrix
Each entry of 𝑦 is the dot product between the corresponding row of A and 𝑥

22. 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

23. Multiple rotations
Define the 2D rotation matrix 𝑅 𝜃 = 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 𝑧 ?

24. Multiple rotations
Define the 2D rotation matrix 𝑅 𝜃 = 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 ) 𝑦

25. Multiple rotations
Define the 2D rotation matrix 𝑅 𝜃 = 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 𝑥 )

26. Multiple rotations
Define the 2D rotation matrix 𝑅 𝜃 = 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 𝑥 )
𝑧 =𝑅 𝜃 2 𝑅 𝜃 1 𝑥 (matrix * is associative)

27. Matrix-matrix multiplication
𝑧 = 𝑅 𝜃 2 𝑅 𝜃 1 𝑥 so 𝑅 𝜃 2 𝑅 𝜃 1 is a 2x2 matrix
𝑎𝑒+𝑏𝑔 𝑎𝑓+𝑏ℎ 𝑐𝑒+𝑑𝑔 𝑐𝑓+𝑑ℎ = 𝑎 𝑏 𝑐 𝑑 𝑒 𝑓 𝑔 ℎ

28. Matrix-matrix multiplication
𝑧 = 𝑅 𝜃 2 𝑅 𝜃 1 𝑥 so 𝑅 𝜃 2 𝑅 𝜃 1 is a 2x2 matrix
𝑎𝑒+𝑏𝑔 𝑎𝑓+𝑏ℎ 𝑐𝑒+𝑑𝑔 𝑐𝑓+𝑑ℎ = 𝑎 𝑏 𝑐 𝑑 𝑒 𝑓 𝑔 ℎ
Verify that 𝑎𝑒+𝑏𝑔 𝑎𝑓+𝑏ℎ 𝑐𝑒+𝑑𝑔 𝑐𝑓+𝑑ℎ ⋅ 𝑥 𝑦 = 𝑎 𝑏 𝑐 𝑑 ⋅ 𝑒 𝑓 𝑔 ℎ ⋅ 𝑥 𝑦 𝑒 𝑓 𝑔 ℎ ⋅ 𝑥 𝑦

29. Rotation matrix-matrix multiplication
𝑧 = 𝑅 𝜃 2 𝑅 𝜃 1 𝑥 so 𝑅 𝜃 2 𝑅 𝜃 1 is a 2x2 matrix
𝑎𝑒+𝑏𝑔 𝑎𝑓+𝑏ℎ 𝑐𝑒+𝑑𝑔 𝑐𝑓+𝑑ℎ = 𝑎 𝑏 𝑐 𝑑 𝑒 𝑓 𝑔 ℎ
𝑎 𝑏 𝑐 𝑑 = cos 𝜃 2 −sin 𝜃 2 sin 𝜃 2 cos 𝜃 2
𝑒 𝑓 𝑔 ℎ = cos 𝜃 1 −sin 𝜃 1 sin 𝜃 1 cos 𝜃 1

30. Rotation matrix-matrix multiplication
𝑧 = 𝑅 𝜃 2 𝑅 𝜃 1 𝑥 so 𝑅 𝜃 2 𝑅 𝜃 1 is a 2x2 matrix
𝑎𝑒+𝑏𝑔 𝑎𝑓+𝑏ℎ 𝑐𝑒+𝑑𝑔 𝑐𝑓+𝑑ℎ = 𝑎 𝑏 𝑐 𝑑 𝑒 𝑓 𝑔 ℎ
𝑎 𝑏 𝑐 𝑑 = 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 )

31. Rotation matrix-matrix multiplication
𝑧 = 𝑅 𝜃 2 𝑅 𝜃 1 𝑥 so 𝑅 𝜃 2 𝑅 𝜃 1 is a 2x2 matrix
𝑎𝑒+𝑏𝑔 𝑎𝑓+𝑏ℎ 𝑐𝑒+𝑑𝑔 𝑐𝑓+𝑑ℎ = 𝑎 𝑏 𝑐 𝑑 𝑒 𝑓 𝑔 ℎ
𝑎 𝑏 𝑐 𝑑 = 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, 𝑹 𝜽 𝟐 𝑹 𝜽 𝟏 =𝑹 𝜽 𝟏 + 𝜽 𝟐

32. 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 𝑝 𝐴 𝑖,𝑘 𝐵 𝑘,𝑗

33. 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., 𝐴 𝑇 𝑖,𝑗 = 𝐴 𝑗,𝑖
𝐴𝐵 𝑇 = 𝐵 𝑇 𝐴 𝑇

34. Consequence: rotation inverse
Since 𝑅 𝜃 2 𝑅 𝜃 1 =𝑅 𝜃 1 + 𝜃 2 …
𝑅 𝜃 𝑅 −𝜃 =𝑅 𝜃−𝜃 =𝑅 0 =𝐼= (the identity matrix)
But 𝑅 −𝜃 =𝑅 𝜃 𝑇
…so a rotation matrix’s inverse is its transpose.

35. Break

36. 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
cos θ -sin θ
sin θ cos θ x y tx ty
Tq(x,y) = +
2D rotation matrix
Affine translation 36

37. Rigid Transformation in 3D
q = (tx,ty,tz,R) with R a 3x3 rotation matrix
Robot R0R3 given in reference frame T0
What’s the new robot Rq? {Tq(x,y,z) | (x,y,z)  R0}
Define rigid transformation Tq(x,y,z) : R3 R3 R x y z tx ty tz
Tq(x,y,z) = +
3D rotation matrix
Affine translation 37

38. 3-D Rigid Rotations r11 r12 r13 r21 r22 r23 r31 r32 r33 det(A) = +1
Orthogonal rows and columns: ATA=I, AAT=I
||Ax|| = ||x|| for any x, L2 norm ||.||
Product of two rotations is a rotation
(0,1,0)
(r11,r21,r31)
(Right-handed coordinate system)
(r13,r23,r33)
(1,0,0)
(1,0,0)
(r12,r22,r32)
(0,0,1)
(0,0,1) 38

39. Coordinate Frames R R-1 = RT Local frame World frame 𝑟 13 𝑟 23 𝑟 33
𝑟 13 𝑟 23 𝑟 33
(0,1,0) R
𝑥 𝑦 𝑧
𝑟 11 𝑟 21 𝑟 31
(1,0,0)
R 𝑥 𝑦 𝑧
𝑟 12 𝑟 22 𝑟 32
(0,0,1)
R-1
= RT

40. Transformation inverse
In 2D and 3D, a rotation inverse is simply a matrix transpose… but what about a rigid transform?
The inverse of a rigid transform is a rigid transform, but it may not be immediately obvious how to represent it in canonical 𝑅𝑥+𝑡 form.
Exercise left for homework 1

41. 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
Directional quantities (e.g., velocity, force) are not affected by the affine translation!
cos θ -sin θ
sin θ cos θ x y tx ty
Tq(x,y) = +
cos θ -sin θ
sin θ cos θ vx vy
Rq(vx,vy) = 41

42. Transform Hell

43. Becoming a master of transformations…
Rule #1: Comparisons between vectors only makes sense if their coordinates are in the same reference frame!
When many vectors / matrices are floating around, keeping track can get very confusing
Tip: use subscripts to denote frame
Rule #2: Converting vectors to a different reference frame require applying a transformation
You need to know the transformation between reference frames
Rule #3: Mind your P’s and D’s
P-P=D, P+D=P, D+D=D, D*c=D
Do not do P+P, P*c!
D’s don’t “feel” translation
Rule #4: if you know the transformation from every frame to the world frame, you can transform between any two frames

44. Homogeneous coordinates
Sorting out positions from directions is somewhat messy, is there a better way?
Homogeneous coordinates:
Add a 3rd component, w
w=1 indicates a point, w=0 indicates a direction
In normal coordinates, [x,y,w]T represents a point [x/w,y/w]T
Directions are “points at infinity”

45. Homogeneous transforms
A 2D rotation can be represented as a 3x3 homogeneous matrix
𝑅(𝜃) 0 0 1
Where 𝑅 𝜃 is the original 2x2 rotation matrix
A 2D rigid transform 𝑇 𝑥 =𝑅 𝜃 𝑥+𝑡 can be represented as a 3x3 homogeneous matrix
𝑅(𝜃) 𝑡 0 1
Verify that multiplication of both points and directions in homogeneous coordinates performs the correct transformation, preserving P- and D-ness

46. Homogeneous transforms in 3D
A point becomes a 4D vector [x,y,z,1], while a direction becomes [x,y,z,0]
A 3D rotation can be represented as a 4x4 homogeneous matrix
𝑅 0 0 1
Where 𝑅 is the original 3x3 rotation matrix
A 3D rigid transform 𝑇 𝑥 =𝑅𝑥+𝑡 can be represented as a 4x4 homogeneous matrix
𝑅(𝜃) 𝑡 0 1

47. Does this solve all our transform confusion?
No, it still doesn’t enforce consistency of reference frames, or prohibit operations like P+P or P*c.

48. Next Time
3D rotations
Readings:
Principles, Appendix E