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

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

Rigid Objects

Biological systems, virtual characters

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

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

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

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

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

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 X axis of T0 gets coords 𝑥 = cos 𝜃 sin 𝜃 , Y axis gets 𝑦 = −sin 𝜃 cos 𝜃 cos q cos q q sin q

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 𝑝 𝑥 ⋅ 𝑥 𝑝 𝑦 ⋅ 𝑦

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 𝑝 𝑥 ⋅ 𝑥 𝑝 𝑦 ⋅ 𝑦

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) 𝑥 = 𝑥 ⋅ 𝑥

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) 𝑥 = 𝑥 ⋅ 𝑥

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 𝑣 . 𝑣 𝑥 ⋅ 𝑣

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 𝜃

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 𝜃 𝑝 𝑥 𝑝 𝑦

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 𝒙

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 𝑥

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 𝑥

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

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 𝑧 ?

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 ) 𝑦

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 𝑥 )

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)

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

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

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

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 )

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, 𝑹 𝜽 𝟐 𝑹 𝜽 𝟏 =𝑹 𝜽 𝟏 + 𝜽 𝟐

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 =𝐼= 1 0 0 1 (the identity matrix) But 𝑅 −𝜃 =𝑅 𝜃 𝑇 …so a rotation matrix’s inverse is its transpose.

Break

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

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

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

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

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

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

Transform Hell

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

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”

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

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

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.

Next Time 3D rotations Readings: Principles, Appendix E