<<

Chapter 2 Math Fundamentals

Part 1 2.1 Conventions and Definitions 2.2 Matrices 2.3 Fundamentals of Rigid Transforms

1 Mobile Robotics - Prof Alonzo Kelly, CMU RI Outline • 2.1 Conventions and Definitions • 2.2 Matrices • 2.3 Fundamentals of Rigid Transforms • Summary

2 Mobile Robotics - Prof Alonzo Kelly, CMU RI Outline • 2.1 Conventions and Definitions – 2.1.1 Notational Conventions – 2.1.2 Embedded Coordinate Frames • 2.2 Matrices • 2.3 Fundamentals of Rigid Transforms • Summary

3 Mobile Robotics - Prof Alonzo Kelly, CMU RI Physical Quantities

• Mechanics is about r properties of / relations b a between objects. • a is “r-related” to b • r property of a relative to b • Example velocity (v) of robot 𝑏𝑏 (r) relative to earth (e): 𝑟𝑟𝑎𝑎 𝑒𝑒 • Relationship is directional 𝑟𝑟 and (often) asymmetric. 𝑣𝑣 𝑏𝑏 𝑎𝑎 𝑟𝑟𝑎𝑎 ≠ 𝑟𝑟𝑏𝑏

4 Mobile Robotics - Prof Alonzo Kelly, CMU RI Properties ? • “r’ is not quite a property of a. – “the” velocity of an object is not 𝑏𝑏 defined. 𝑟𝑟𝑎𝑎 z • It’s a property of a relative to b. x y • a and b are real objects. • In rare instances, we do not need a b. z

– unit vectors always of length 1. y x

5 Mobile Robotics - Prof Alonzo Kelly, CMU RI Vectors, Matrices, and

• With one exception (e.g. clight) all require a datum (def’n of zero). • May be scalars (density), vectors (velocity), tensors (_?_). – All are tensors of varying order. • We write:

• The vectors𝑏𝑏𝑏𝑏𝑏𝑏𝑏𝑏 at least𝑏𝑏𝑏𝑏 𝑏𝑏𝑏𝑏can be𝑏𝑏𝑏𝑏 of𝑏𝑏𝑏𝑏 1, 2,𝑏𝑏 𝑏𝑏or𝑏𝑏𝑏𝑏 3 𝑏𝑏𝑏𝑏𝑏𝑏𝑏𝑏 𝜌𝜌 . 𝑟𝑟⃑ 𝑣𝑣⃑ 𝑎𝑎⃑ 𝐼𝐼

6 Mobile Robotics - Prof Alonzo Kelly, CMU RI Frames of Reference and Coordinate Systems

• Objects of interest are real: z wheels, sensors, obstacles. x y • Abstract them by sets of axes fixed to the body. • These axes: – Have a state of motion Reference Frame – Can be used to express vectors. • Call them coordinate frames.

7 Mobile Robotics - Prof Alonzo Kelly, CMU RI Coordinate Frames • Points possess but not :

Particle (orientation undefined) • Rigid Bodies possess position and orientation:

Ball (can rotate)

• A : – does not have one position. – does have one orientation

8 Mobile Robotics - Prof Alonzo Kelly, CMU RI Outline • 2.1 Conventions and Definitions – 2.1.1 Notational Conventions – 2.1.2 Embedded Coordinate Frames • 2.2 Matrices • 2.3 Fundamentals of Rigid Transforms • Summary

9 Mobile Robotics - Prof Alonzo Kelly, CMU RI Vectors and Coordinates • Many laws of relate vectors and hold regardless of coordinate system. • Notation: – is expressed in some coordinate system.

𝑟𝑟 – is coordinate system independent.

𝑟𝑟⃑ c b a

10 Mobile Robotics - Prof Alonzo Kelly, CMU RI Vectors and Coordinates • Vectors of physics are coordinate system independent: c = a + b

c Tool b a World • Addition is defined geometrically.

11 Mobile Robotics - Prof Alonzo Kelly, CMU RI Vectors and Coordinates • Vectors of are coordinate system dependent: c = a + b

cx ax bx = + c a b y y y • Addition is defined algebraically. • A relation to physical vectors (directed segments) requires a coordinate system.

12 Mobile Robotics - Prof Alonzo Kelly, CMU RI Free and Bound Transformations • Distinguishes what happens when the reference frame changes. • Bound: the vector may change and its expression may change. – Transformation of (physics). • Free: the vector remains the same and its expression may change – Transformation of coordinatesp (). a b rp rp a a b rp

13 Mobile Robotics - Prof Alonzo Kelly, CMU RI Notational Conventions

o c d r c r o d • r  relationship / property • o  object to which property is attributed • d  object serving as datum • c  object providing the coordinate system

14 Mobile Robotics - Prof Alonzo Kelly, CMU RI Box 2.1 Notation for Physical Quantities • : the r property of a expressed in the default coordinate system associated with object a. 𝑟𝑟𝑎𝑎 • : the r property of a relative to b in coordinate system𝑏𝑏 independent form. 𝑟𝑟⃑𝑎𝑎 • : the r property of a relative to b expressed in the default𝑏𝑏 coordinate system associated with object b. 𝑟𝑟𝑎𝑎 • : the r property of a relative to b expressed in the default𝑐𝑐 𝑏𝑏 coordinate system associated with object c. 15 𝑟𝑟𝑎𝑎 Mobile Robotics - Prof Alonzo Kelly, CMU RI 16 Mobile Robotics - Prof Alonzo Kelly, CMU RI Sub/Super Scripts – Physics Vectors • Leading subscripts denote the frame/ object possessing the vector quantity: vball • Leading superscripts denote the frame/object with respect to which the quantity is measured (i.e. the datum): world train world vball = vball + vtrain

World

17 Mobile Robotics - Prof Alonzo Kelly, CMU RI Sub/SuperScripts – LA Vectors • Leading subscripts denote the object frame possessing the quantity: v wheel • Leading superscripts denote the datum (also the implied coordinate system within which the quantity is expressed). world vwheel • Trailing superscripts denote the coordinate system, and leading denotes datum when necessary. body wheel vworld

18 Mobile Robotics - Prof Alonzo Kelly, CMU RI Notational Conventions • Position vectors: r = T xyz

• Also sometimes as r or as r to emphasize it is a vector. • Matrices: t t t xx xy xz T = tyx tyy tyz

tzx tzy tzz

19 Mobile Robotics - Prof Alonzo Kelly, CMU RI Why All The Fuss ? • Accelerometer: acceleration of the sensor wrt inertial space: i as • Strapdown: acceleration of the sensor wrt inertial space referred to b i body coordinates: as • Nav Solution: Acceleration of the body wrt earth referred to earth e e coordinates: ab

20 Mobile Robotics - Prof Alonzo Kelly, CMU RI Why All The Fuss ? • WMR are much easier to do in

the body frame. e vfr • Velocity of the front right wheel wrt the earth (“world”) frame: e vfr • Velocity of front right wheel wrt earth e referred to body coordinates: b e vfr

21 Mobile Robotics - Prof Alonzo Kelly, CMU RI Why All The Fuss ? • Coordinate system may be unrelated to either b w ve vfr fr the object or the datum. – So you need a third symbol to be precise.

e

w

22 Mobile Robotics - Prof Alonzo Kelly, CMU RI Converting Coordinates

b b a r = Ta r

b • We will see later that T notation satisfies our conventions where it meansa the ‘T’ property of ‘object’ a wrt ‘object’ b.

23 Mobile Robotics - Prof Alonzo Kelly, CMU RI Outline • 2.1 Conventions and Definitions • 2.2 Matrices • 2.3 Fundamentals of Rigid Transforms • Summary

24 Mobile Robotics - Prof Alonzo Kelly, CMU RI Tensors • For our purposes, these are multidimensional arrays: • Consider T(I,j,k) to be a 3D “box” of . • Suppose it is 3X3X3, then there are three “slices” extending out of the page.

2 3.6 7 35– 12 7 9.2 18 T1 = T2 = T3 = 8– 4.3 0 42– 1 84– 0 13

T213[],, =T2[][]1 []3 ==t213 12

25 Mobile Robotics - Prof Alonzo Kelly, CMU RI 25 Some Notation

• Block Notation: A A A = 11 12 A21 A22

A = • Notation: aijk – [ .. ] means a set that can be arranged in a rectangle that is ordered in each .

26 Mobile Robotics - Prof Alonzo Kelly, CMU RI 26 Operations ⋅ • Vector : ab= ∑ak bk k – Also written T • multiplication: a b C== AB[] c =a b – Dot product of i-th row and ij ∑ ik kj j-th column k

aybz – azby c== ab× • azbx – axbz X – Also written: c== ab× a b axby – aybx

0a– z ay aX = – ‘Skew” matrix az 0a– x –ay ax 0

27 Mobile Robotics - Prof Alonzo Kelly, CMU RI 27 Operations

• 2.2.1.7 Outer Product:

axbx axby axbz T c== ab a b a b a b y x y y y z azbx azby azbz

• 2.2.1.8 Block Multiplication: A A B B A = 11 12 B = 11 12 A 21 A22 B21 B22

A B + A B A B + A B AB = 11 11 12 21 11 12 12 22 A21B11 + A22 B21 A21 B12 + A22 B22

28 Mobile Robotics - Prof Alonzo Kelly, CMU RI 28 2.2.1.9 Linear Mappings • Of course, this is: y= Ax – “every element of y depends on every element of x in a linear manner”. = • Two views: – “A operates on x”: y is the list of mX1 mXn projections of x on each row of A.

– “x operates on A”: y is a weighted nX1 sum of the columns of A. x is the weights. • A turns x into y or x collapses A’s rows to produce y

29 Mobile Robotics - Prof Alonzo Kelly, CMU RI 29 2.2.2 Matrix Functions … • Function of a :

a11 ()t a12 ()…t At()==() a21 ()t a22 ()…t aij t … …… • Function of a vector:

a11 ()x a12 (x )… Ax()==() a21 ()x a22 (x )… aij x … ……

30 Mobile Robotics - Prof Alonzo Kelly, CMU RI 30 Exponentiation • Powers of matrices automatically commute: 3 2 2 A ===AA() ()A A AAA

• Hence, we can define matrix “polynomials”:

2 Y= AX + BX+ C

• Not so useful in practice but: – Y = aX2 +bX +c (scalar coefficients) is super useful.

31 Mobile Robotics - Prof Alonzo Kelly, CMU RI 31 Arbitrary Functions of Matrices • Recall the Taylor Series: 2 3 df x2 d f x3d f fx()= f0()++x -----  +-----  +… dx 2! dx 3! dx 0 0 0 • Taylor series for exponential function: 2 3 x x e ==exp()x 1++++ x x------… 2! 3!

• Hence, define the matrix exponential as:

A2 A 3 exp()A = I++++ A ------… 2! 3!

32 Mobile Robotics - Prof Alonzo Kelly, CMU RI 32 2.2.3 Matrix Inversion & Inverse Mapping • Matrix inverse defined s.t.:

–1 A AI=

• Therefore:

–1 –1 A yA==Ax x

xA= –1y

33 Mobile Robotics - Prof Alonzo Kelly, CMU RI 33 2.2.3.2 Determinant

• Scalar-valued function of a det() A ==ab ad– cb matrix. cd • Matrix not invertible if distinct Matrix A inputs map to same output. • Determinant measures: – Volume spanned by rows of A. – Ratio of the volumes spanned by (c,d) two input and the associated two output vectors. (a,b)

34 Mobile Robotics - Prof Alonzo Kelly, CMU RI 34

2.2.3.3 Rank • Rank = “dimension of largest invertible submatrix” • Unlike determinant, defined for nonsquare matrices. • A nonsquare matrix can have a rank no larger than the smaller of its two dimensions. • The rank of a matrix product cannot exceed the minimum of the ranks of the two operands. • For an mXn matrix A with (m <= n) – Rank = m  “is of full rank” – Rank < m  “is rank deficient” • For an n X n matrix A: – Rank = n  “nonsingular”, “invertible” – Rank < n  “singular”, “noninvertible”

35 Mobile Robotics - Prof Alonzo Kelly, CMU RI 35 2.2.3.4 Positivity • A square matrix A is called “positive definite” if: T x Ax> 0 ∀ x≠ 0 • Matrix equivalent to positive scalars: – E.g. sum of two pos def. matrices is pos. def. • Covariance, inertia, are always positive definite (in absence of bugs). • f(x)=xTAx is a parabloid in n dimensions.

36 Mobile Robotics - Prof Alonzo Kelly, CMU RI 36 2.2.3.5 Homogeneous Linear Systems • Special form of system: = 0

• Is called a homogeneous𝐴𝐴𝑥𝑥 system. • When is nonsingular, the solution is, of course: = 0=0 𝐴𝐴 • If is singular, there are− an1 infinite of 𝑥𝑥 𝐴𝐴 nonzero solutions and is in the nullspace of A (see𝐴𝐴 below for more on nullspace). 𝑥𝑥

37 Mobile Robotics - Prof Alonzo Kelly, CMU RI 2.2.3.6 Eigenvalues and Eigenvectors • The vector is an eigenvector of when it satisfies: 𝑒𝑒 = 𝐴𝐴 • For some scalar called the eigenvalue associated 𝐴𝐴𝑒𝑒 𝜆𝜆𝑒𝑒 with . 𝜆𝜆 • To solve, rewrite first as: 𝑒𝑒 = 0 • Therefore, for this homogeneous system, nonzero 𝜆𝜆𝐼𝐼 − 𝐴𝐴 𝑒𝑒 implies: = 0

38𝑒𝑒 Mobile Robotics - Prof Alonzo Kelly, CMU RI 𝑑𝑑𝑑𝑑𝑑𝑑 𝜆𝜆𝜆𝜆 − 𝐴𝐴 2.2.4 Subspaces • Consider: = ; × • Range or columnspace of , denoted𝑚𝑚 C𝑛𝑛 is the set of all possible values for𝑦𝑦 𝐴𝐴 𝑥𝑥 ×𝐴𝐴. ∈ ℝ – equivalently all possible linear𝑚𝑚𝐴𝐴 1 combinations𝐴𝐴 of columns of A 𝑦𝑦 ∈ ℝ • The rowspace of , denoted R is the set of all vectors × for which = 0. – equivalently𝑛𝑛 1 all possible𝐴𝐴 linear combinations𝐴𝐴 of the rowsof A. 𝑥𝑥 ∈ ℝ 𝑦𝑦 𝐴𝐴𝑥𝑥 ≠ • The nullspace of , denoted N is the set of all vectors × for which = = 0. – Its rank𝑛𝑛 1 is called𝐴𝐴 nullity. 𝐴𝐴 𝑥𝑥 ∈ ℝ 𝑦𝑦 𝐴𝐴𝑥𝑥

39 Mobile Robotics - Prof Alonzo Kelly, CMU RI 2.2.4 Rank Nullity Theorem • Every vector in the rowspace is orthogonal to every vector in the nullspace: ( ) ( ) • The union of these two subspaces of is . 𝑅𝑅 𝐴𝐴 ⊥ 𝑁𝑁 𝐴𝐴 = 𝑛𝑛 𝑛𝑛 ℝ ℝ • The dimensions of these two sum𝑛𝑛 to m: 𝑅𝑅 𝐴𝐴 +∪ 𝑁𝑁 𝐴𝐴 ℝ =

𝑟𝑟𝑟𝑟𝑟𝑟𝑟𝑟 𝐴𝐴 𝑛𝑛𝑛𝑛𝑛𝑛𝑛𝑛𝑛𝑛𝑛𝑛𝑛𝑛 𝐴𝐴 𝑛𝑛

40 Mobile Robotics - Prof Alonzo Kelly, CMU RI 2.2.5.3 Blockwise Matrix Elimination

n m 1 1 • Given: AB xA yA n nn× n = = m mn× m CD xB yB

• Assuming A is invertible, multiply first block row by -1 CA : –1 x CA–1y C CA B A = A x CD B yB • Subtract two blocks of m rows to produce: –1 –1 ()D– CA B xB = yB – CA yA

• Solve for xB and substitute into original 1st equation to get xA.

41 Mobile Robotics - Prof Alonzo Kelly, CMU RI 41 2.2.5.4 Matrix Inversion Lemma • This is: –1 –1 –1 –1 –1 –1 –1 []A– BD C = A + A B[] D– CA B CA • Derived in the text using block matrix inversion. • The matrix inversion on left is nXn. The one on the right is mXm. – Therefore, the inversion lemma is less work. – Often m=1 so its a lot less work. – Special case is the Sherman–Morrison formula often used to give a rank 1 update to an inverse. • Kalman filter is based on this.

42 Mobile Robotics - Prof Alonzo Kelly, CMU RI 42 2.2.6.2 Expansion Operations • Derivative of a matrix with respect to a vector is a 3rd order tensor.

∂Yx() ∂ = y ()x ∂x ∂x ij k

• Each k is a different matrix [yij]: • Use this to perturb a matrix-valued function:

∂Yx() ∆Y = ∂x ∆x

43 Mobile Robotics - Prof Alonzo Kelly, CMU RI 43 2.2.6.4 Product Rules - 1 • Derivative of a matrix product w.r.t a scalar: ∂ ∂ ∂ ∂ Cx()=={}Ax()Bx() {}Ax()Bx()+ Ax() {}Bx() ∂x ∂x ∂x ∂x

• Example:

T d{}xt() Ax() t T = x· Ax+ xTAx· dt

44 Mobile Robotics - Prof Alonzo Kelly, CMU RI 44 2.2.6.7 Product Rules - 2 • Derivative of a matrix product w.r.t a vector:

∂{}Ax()Bx() ∂ ∂ Cx()==Ax()Bx()+ Ax() Bx() ∂x ∂x ∂x

• Examples:

T T ∂{}a x T ∂{}x A T = a = A ∂x ∂x T ∂{}x ∂{}Ix ∂{}x a T ==I = a ∂x ∂x ∂x T T T ∂{}x Ax ∂{}x ∂{}x I = xTAT + xT A ==I ∂x ∂x ∂x

45 Mobile Robotics - Prof Alonzo Kelly, CMU RI 45 2.2.6.8 Names and Notation for Derivatives

46 Mobile Robotics - Prof Alonzo Kelly, CMU RI Outline • 2.1 Conventions and Definitions • 2.2 Matrices • 2.3 Fundamentals of Rigid Transforms – 2.3.1 Definitions – 2.3.2 Why Homogeneous Transforms – 2.3.3 Semantics and Interpretations • Summary

47 Mobile Robotics - Prof Alonzo Kelly, CMU RI 2.3.1 Affine Transformation • Most general linear transformation

x r r x t 2 = 11 12 1 + 1 y2 r21 r22 y1 t2

• R’s and t’s are the transform constants • Can be used to effect , , scale, reflections, and shear. • Preserves linearity but not (hence, not areas or angles).

48 Mobile Robotics - Prof Alonzo Kelly, CMU RI 2.3.1 Homogeneous Transformation

• Set t1 = t2 = 0: Homogeneous

x2 r11 r12 x1 = + nothing

y2 r21 r22 y1

• r’s are the transform constants • Can be used to effect rotation, scale, reflections, and shear (not translation). • Preserves linearity but not distance (hence, not areas or angles).

49 Mobile Robotics - Prof Alonzo Kelly, CMU RI 2.3.1 Orthogonal Transformation • Looks the same … but:

x r r x r11 r12 + r21 r22 = 0 2 = 11 12 1 r11r11 + r21 r21 = 1 y2 r21 r22 y1 r r + r r = 1 12 12 22 22 • But: – Can be used to effect rotation. – Preserves linearity and distance (hence, areas and angles).

50 Mobile Robotics - Prof Alonzo Kelly, CMU RI Outline • 2.1 Conventions and Definitions • 2.2 Matrices • 2.3 Fundamentals of Rigid Transforms – 2.3.1 Definitions – 2.3.2 Why Homogeneous Transforms – 2.3.3 Semantics and Interpretations • Summary

51 Mobile Robotics - Prof Alonzo Kelly, CMU RI 2.3.2 • Coordinates which are unique up to a scale factor. i.e x= 6x==– 12x 3.14x = same thing • The numbers in the vectors are not the same but we interpret them to mean the same thing (in fact, the thing whose scale factor is unity).

T r˜ = xyzw w=1 w T x y z r˜ = ------1 w w w

52 Mobile Robotics - Prof Alonzo Kelly, CMU RI Pure Directions • Its also possible to represent pure directions – Pure in the sense they “are everywhere” (i.e. have no position and cannot be moved). • We use a scale factor of zero to get a pure direction: x y d = z 0 • It will shortly be clear why this works.

53 Mobile Robotics - Prof Alonzo Kelly, CMU RI Why Bother? • Points in 3D can be rotated, reflected, scaled, and sheared with 3 X 3 matrices….

x' txx txy txz x txxxt++xyytxzz r' = ==Tr = y' tyx tyy tyz y tyxxt++yyytyzz z' z tzx tzy tzz tzxxt++zy ytzzz • But not translated. x1 ∆x r'== r + ∆r + y1 ∆y ∆ z z1 • What 3X3 matrix is Trans(∆r)?

54 Mobile Robotics - Prof Alonzo Kelly, CMU RI Trick: Move to 4D

x ∆x 100∆x x ∆ ∆ r'== r+ ∆r y + y =010 y y =Trans()∆r r z ∆z 001∆z z 1 1 000 1 1

x= 1x×∆+ x y= 1y×∆+ y z= 1z×∆+ z

• The scale factor in the vector is used to add a scaled amount of the 4th matrix column.

55 Mobile Robotics - Prof Alonzo Kelly, CMU RI HT Matrix Format

input x y z w

x y output z

w

56 Mobile Robotics - Prof Alonzo Kelly, CMU RI Outline • 2.1 Conventions and Definitions • 2.2 Matrices • 2.3 Fundamentals of Rigid Transforms – 2.3.1 Definitions – 2.3.2 Why Homogeneous Transforms – 2.3.3 Semantics and Interpretations • Summary

57 Mobile Robotics - Prof Alonzo Kelly, CMU RI Trig Function Shorthand • sin(θ)  sθ • cos(θ)  cθ • tan(θ)  tθ

• sin(θ1)cos(θ2)  sθ1cθ2  s1c2

• sin(θ1+θ2)  sθ1θ2  s12

58 Mobile Robotics - Prof Alonzo Kelly, CMU RI Operators • Mapping: –  Point ’ (both expressed in same coordinates)

x

p’ p

z

y

59 Mobile Robotics - Prof Alonzo Kelly, CMU RI Operators

x 100u Transuvw(),, = 010v p’ 001w p 0001 z • Note capital T in Trans(). y

60 Mobile Robotics - Prof Alonzo Kelly, CMU RI Operators

x 10 0 0 Rotx()φ = 0cφ –sφ 0 p’ 0sφ cφ 0 p 00 0 1 φ z • Note capital R in Rotx(). y

61 Mobile Robotics - Prof Alonzo Kelly, CMU RI Operators

y cθ 0sθ 0 Roty()θ = 0 100 p’ –sθ 0cθ 0 p 0 001 θ x • Note capital R in Roty(). z

62 Mobile Robotics - Prof Alonzo Kelly, CMU RI Operators

z cψ –sψ 00 ψ ψ Rotz()ψ = s c 00 p’ 0 0 10 p 0 0 01

ψ y • Note capital R in Rotz(). x

63 Mobile Robotics - Prof Alonzo Kelly, CMU RI Compound Operators • Mapping: – Point  Point′ (both expressed in same coordinates) • Compound mapping: – Point′  Point′′ (still in same coordinates)

• Operators have fixed axis compounding semantics.

64 Mobile Robotics - Prof Alonzo Kelly, CMU RI Example: Operating on a Point • A point at the is translated along the y axis by ‘v’ units and then the resulting point is rotated by 90 degrees around the x axis.

o''= Rotx()π ⁄ 2 Trans() 0,, v 0 o T o'' = 00v1 z 1000 1000 0 1000 0 0 o'' =00– 10 010v 0 ==00– 10 v 0 0100 0010 0 0100 0 v 0001 0001 1 0001 1 1

y Result v o' = T x 0v01 65 Mobile Robotics - Prof Alonzo Kelly, CMU RI Example: Operating on a Direction • The y axis is “translated” along the y axis by ‘v’ units and then rotated by 90 degrees around the x axis.

j '= Rotx()π ⁄ 2 Trans() 0,, v 0 j T j'= 0010 z 1000 1000 0 1000 0 0 00– 10 010v 1 00– 10 1 0 j' = == 0100 0010 0 0100 0 1 0001 0001 0 0001 0 0

Same! y • Having a zero scale factor T x j = 0100 disables translation. 66 Mobile Robotics - Prof Alonzo Kelly, CMU RI HTs as Coordinate Frames • The columns of the identity HT can be considered to represent 3 directions and a point – the coordinate frame itself.

3 directions{ point {

T T o = 0001 k = 0010 z 1000 0100 = = I T ijko i = 1000 0010 y 0001 T x j = 0100

67 Mobile Robotics - Prof Alonzo Kelly, CMU RI Example: Operating on a Frame • Each resulting column of this result is the transformation of the corresponding column in the original identity matrix … . T I '= Rotx()π ⁄ 2 Tra ns() 0,, v 0 I 01– 00 z T j’ 0010 k’ 10 0 0 1000 1000 10 0 0 I' = 00– 10 010v 0100 = 00– 10 i’ 01 0 0 0010 0010 01 0 v y 00 0 1 0001 0001 00 0 1 x 68 Mobile Robotics - Prof Alonzo Kelly, CMU RI Epiphany 1: HTs are Operators, and Operands, and Displacements. • 1) The operator that moves points as desired also moves axes in the same way. • But because the columns of an input matrix are treated independently in … • 2) … the operator also moves entire frames (4 columns) in one shot when you express them as a matrix. – Note that the HT matrix can now be either operator or operand. • As Operand: But because frames can be embedded to track the motions of rigid bodies. – We can use this idea to computationally track the position and orientation of rigid bodies…. • As Operator: Every orthogonal matrix can be viewed as a displacement in translation and rotation. – Can be visualized as one set of axes located with respect to another set.

69 Mobile Robotics - Prof Alonzo Kelly, CMU RI Epiphany 2 : Operator = Transformed Old Frame • Notice the result is: Rotx( /2) [ (0, , 0) ] • Which is: [Rotx𝜋𝜋 ( /∗2)𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇 (𝑣𝑣0, , 0∗)𝐼𝐼]

• Which is the compound𝜋𝜋 ∗ 𝑇𝑇operator:𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇 𝑣𝑣 ∗ 𝐼𝐼 [Rotx( /2) (0, , 0)]

• When the operand𝜋𝜋 is the∗ identity,𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇 the𝑣𝑣 result is also the operator itself.

70 Mobile Robotics - Prof Alonzo Kelly, CMU RI Epiphany 3 : Operator = Transform • Because the operator, like all operators, expresses the new frame in the coordinates of the original frame…. • 1) The operator has columns that express the new axes in the coordinates of the old ones. • Because = = + + (where is 1st col of A)… 1 1 2 2 1 • And because𝑦𝑦 the𝐴𝐴𝑥𝑥 columns𝑎𝑎 𝑥𝑥 of𝑎𝑎 the𝑥𝑥 operator⋯ are the𝑎𝑎 transformed unit vectors and origin…. • Then the operator [Rotx( /2) (0, , 0)] must convert coordinates from the primed frame to the unprimed frame. 𝜋𝜋 ∗ 𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇𝑇 𝑣𝑣 • See below for more..

71 Mobile Robotics - Prof Alonzo Kelly, CMU RI Conversion of • Operator: the result is the movement of frame a’s unit vectors (“basis”) to those of frame b. • So the result must express the unit vectors of frame b in coordinates of frame a. 10 0 0 ka a a 00– 10 I b ==Rotx()π ⁄ 2 Trans0v0(),, I a jb 01 0 v k b 00 0 1 ib • We have converted coordinates of ja the basis b from frame b to frame a. ia 72 Mobile Robotics - Prof Alonzo Kelly, CMU RI Converting Frames of Reference • Converting frames is about expressing the same physical point with respect to a new origin and set of unit vectors.

zb za

yb ya x b x b a a rp rp

p

73 Mobile Robotics - Prof Alonzo Kelly, CMU RI Converting Coordinates • Consider a general point expressed relative to frame b in the coordinates of frame b. b b b b b b b b By Definition: rp = xp ()ib ++yp ()jb zp ()kb +ob • The unit vectors can be expressed in any coordinate system we like. Choose a. Origin change a b a b a b a a rp = xp ()ib ++yp ()jb zp ()kb +ob

a b a b a b a a rp = xp ()Ib ib ++yp ()Ibjb zp ()Ibkb +Ibob

74 Mobile Robotics - Prof Alonzo Kelly, CMU RI Converting Coordinates b • This is: r a a []b ()b b ()b b()b b p rp = Ib xp ib ++yp jb zp kb +ob • Or more simply: a a b r = I r a p b p • Because I b converts the coordinates of the basis, it converts to coordinates of an arbitrary vector too: – because an arbitrary vector is just a linear combination of the basis vectors. – and matrices are linear operators

75 Mobile Robotics - Prof Alonzo Kelly, CMU RI Operator/Transform • The homogeneous transform that moves frame ‘a’ into coincidence with frame ‘b’ (operator) also converts the coordinates (transform) of points in the opposite direction - from frame ‘b’ to frame ‘a’. • Because, of the opposite direction semantics, its sometimes more convenient to use the matrices which convert coordinates from frame ‘a’ to frame ‘b’. – These are just the matrix inverses.

76 Mobile Robotics - Prof Alonzo Kelly, CMU RI Aligning Operations • We can ask of any two frames: – What operations, applied to one frame, bring it into coincidence with the other. • To formulate the aligning operations • ……. is equivalent to formulating the coordinate transformation. • One of the biggest ideas in 3D kinematics.

77 Mobile Robotics - Prof Alonzo Kelly, CMU RI Transforms • Mapping: – Point a  Point b (same physical point) – Think now of moving frame a into coincidence with frame b. zb za

p

yb ya xb xa 78 Mobile Robotics - Prof Alonzo Kelly, CMU RI Transforms

100– u 010– v zb transuvw(),, = za 001– w 000 1

p (u,v,w) are defined as the y position of the origin of b b wrt a expressed in frame ya a coords. x b • Note lowercase t in trans(). xa 79 Mobile Robotics - Prof Alonzo Kelly, CMU RI Note • and theta may need to be swapped on the following slides.

80 Mobile Robotics - Prof Alonzo Kelly, CMU RI Transforms

1 0 00 0cθ sθ 0 x ,x rotx()θ = a b 0s– θ cθ 0 0 0 01

p zb θ za θ • Note lowercase r in rotx(). ya yb 81 Mobile Robotics - Prof Alonzo Kelly, CMU RI Transforms

cφ 0s– φ 0 rot y()φ = 01 0 0 ya,yb sφ 0cφ 0 00 0 1

p xb φ xa φ • Note lowercase r in roty(). za zb 82 Mobile Robotics - Prof Alonzo Kelly, CMU RI Transforms

cψ sψ 00 ψ ψ rotz()ψ = –s c 00 za,zb 0 0 10 0 0 01 p yb ψ ya ψ • Note lowercase r in rotz(). xa xb 83 Mobile Robotics - Prof Alonzo Kelly, CMU RI Compound Transforms • Mapping: – Point a  Point b (same physical point) – Result expressed in frame b • Compound mapping – Point b  Point c (same physical point) – Result now expressed in frame c. • Transforms have moving axis compounding semantics. – Result is not expressed in “the original frame” but rather in the last one.

84 Mobile Robotics - Prof Alonzo Kelly, CMU RI Example: Compound Transformation • The origin of frame b has coordinates [0 0 v 1]T in frame a. Prove it. • Takes two fundamental operations. Compound transforms to convert ob  oa. zi yi a b z ob = trans() 0,, 0– v rotx()–π ⁄ 2 ob a xi Intermediate frame yb zb 1000 10 0 0 0 10 0 0 0 0 v a 0100 00– 10 0 00– 10 0 0 xb ob = == 001v 01 0 0 0 01 0 v 0 v ya 0001 00 0 1 1 00 0 1 1 1 xa 85 Mobile Robotics - Prof Alonzo Kelly, CMU RI Format of HTs o n i o r

Rotation t t i Matrix c e o s V P

Perspective Scale

We will not use the perspective part much

86 Mobile Robotics - Prof Alonzo Kelly, CMU RI Inverse of a HT

-1

R p RT -RTp =

0 0 0 1 0 0 0 1

87 Mobile Robotics - Prof Alonzo Kelly, CMU RI Duality Theorem • Note that transforms and operators of the same name are (matrix) inverses: –1 trans00v(),, = Tra ns() 0,, 0 v –1 rotx()θ = Rotx()θ –1 roty()φ = Roty()φ –1 rotz()ψ = Rotz()ψ • This implies that a sequence of transforms in one order (say, left to right) is identical to the same sequence of operators in the opposite order. • The latter view is traditional in robotics. • Without loss of generality, we will use operators only from now on.

88 Mobile Robotics - Prof Alonzo Kelly, CMU RI Outline • 2.1 Conventions and Definitions • 2.2 Matrices • 2.3 Fundamentals of Rigid Transforms – 2.3.1 Definitions – 2.3.2 Why Homogeneous Transforms – 2.3.3 Semantics and Interpretations • Summary

89 Mobile Robotics - Prof Alonzo Kelly, CMU RI Summary • Tensors are just matrices with 3 or more indices. • Arbitrary functions on matrices can be defined. • The row space is orthogonal to the null space. • Gaussian Elimination can be performed blockwise to express solution to big problems in terms of solutions to small problems. • Matrix valued functions can be differentiated with respect to scalars and vectors. – Layout of resulting tensor is implicit in the latter case.

90 Mobile Robotics - Prof Alonzo Kelly, CMU RI 90 Summary • An operator formed from an orthogonal HT preserves and hence is rigid. • Homogeneous Transforms are: – Operators – Transforms – Frames • They can be both the things that operate on other things and the things operated upon.

91 Mobile Robotics - Prof Alonzo Kelly, CMU RI 91 Summary • Such a 4X4 operator matrix has these properties. – It rotates and/or translates points and directions and hence rotates and translates coordinate frames. – Its columns represent the unit vectors and origin of the result of operating on a coordinate frame expressed in the coordinates of the original frame. – It converts coordinates of points and directions from the result to the original frame.

92 Mobile Robotics - Prof Alonzo Kelly, CMU RI 92 Summary • Everything is relative. There is no way to distinguish moving a point “forward” from moving the coordinate system “backward”.

• In both cases, the resulting (red) point has the same relationship to the redframe.

93 Mobile Robotics - Prof Alonzo Kelly, CMU RI 93