Lecture 7 Nonholonomic Constraint II Representing Rotation I

Lecture 7 Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Lecture 7 Frobenius’s theorem

Kinematic Nonholonomic constraint II representation: goals, overview

Representing Rotation I Spatial rotations Preview Axis-angle Rodrigues’s formula Rotation matrices Matthew T. Mason Matrix exponential Euler angles

Mechanics of Manipulation Lecture 7 Today’s outline Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Nonholonomic constraint II Frobenius’s theorem Frobenius’s theorem

Kinematic representation: Kinematic representation: goals, overview goals, overview Spatial rotations Preview Axis-angle Spatial rotations Rodrigues’s formula Rotation matrices Preview Matrix exponential Axis-angle Euler angles Rodrigues’s formula Rotation matrices Matrix exponential Euler angles Lecture 7 Involutive distribution Nonholonomic constraint II Representing Rotation I Definition (Involutive) Nonholonomic constraint II A distribution is involutive if it is closed under Lie brackets. Frobenius’s theorem Kinematic representation: Definition (Involutive closure) goals, overview Spatial rotations The involutive closure of a distribution ∆ is the closure ∆ Preview Axis-angle of the distribution under Lie bracketing. Rodrigues’s formula Rotation matrices Matrix exponential Euler angles I Given a distribution spanned by a set of vector fields, take Lie brackets for all pairs of vector fields.

I If you get a vector ﬁeld not previously in the span, add it.

I Repeat until you get nothing new. Lecture 7 Frobenius’s theorem Nonholonomic constraint II Representing Theorem (2.8, Frobenius’s theorem) Rotation I A regular distribution is integrable if and only if it is Nonholonomic involutive. constraint II Frobenius’s theorem

Kinematic representation: Proof. goals, overview

Spatial rotations I (integrable → involutive.) Take the Taylor series of Preview Axis-angle parallel parking as a function of . Second order Rodrigues’s formula Rotation matrices terms are Lie brackets! If the distribution is involutive, Matrix exponential the Lie brackets must also be contained in the Euler angles distribution.

I (involutive → integrable). Too involved for us. By induction over dimension.

Restating Frobenius’s theorem: A set of constraints is nonholonomic ↔ parallel parking is useful. Lecture 7 Lessons of nonholonomic constraint Nonholonomic constraint II Representing Rotation I I Robots usually have less motors than task freedoms. There will be constraints. Nonholonomic constraint II I A holonomic constraint is a constraint on Frobenius’s theorem conﬁguration: it says there are places you cannot go. Kinematic representation: That is a reduction in freedoms. That’s (usually) bad. goals, overview

I A nonholonomic constraint is a constraint on velocity: Spatial rotations Preview there are directions you cannot go. But you can still Axis-angle Rodrigues’s formula get wherever you want. That’s (usually) good! Rotation matrices Matrix exponential Euler angles I Parallel parking is general. If you want to move in the constrained direction, pick a pair of controls and interleave oscillations. Or do it mathematically with Lie brackets.

I If parallel parking doesn’t help, you are truly stuck on the leaf of a foliation. Rearrange your motors, or buy more. Lecture 7 Kinematic representation. Readings, etc. Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Frobenius’s theorem

Kinematic representation: I We are starting chapter 3 of the text goals, overview I Lots of stuff online on representing rotations Spatial rotations Preview Axis-angle I Murray, Li, and Sastry for matrix exponential Rodrigues’s formula Rotation matrices I Roth, Crenshaw, Ohwovoriole, Salamin, all cited in Matrix exponential text Euler angles Lecture 7 Analytic geometry Nonholonomic constraint II Representing So far, Euclidean geometry. Why? Rotation I

Nonholonomic I Insight constraint II Frobenius’s theorem

I Visualization Kinematic representation: I Economy of expression goals, overview Spatial rotations Preview Axis-angle Now Cartesian, analytic geometry. Why? Rodrigues’s formula Rotation matrices Matrix exponential Euler angles I Beyond 2D, beyond 3D. We need to work with high dimensional conﬁguration spaces!

I For implementation

I For additional insight

The best of all possible worlds: use both. Understand geometrical or physical meaning for all terms. Lecture 7 Agenda for kinematic representation Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Frobenius’s theorem

Kinematic Following the kinematic agenda: representation: goals, overview

I Planar displacements. No. Skipping this. Spatial rotations Preview Axis-angle I Spherical displacements Rodrigues’s formula Rotation matrices I Spatial displacements Matrix exponential Euler angles I Constraints Lecture 7 Why representing rotations is hard. Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II I Rotations do not commute. Vectors are out. Frobenius’s theorem Kinematic I For computation we like to represent things with real representation: goals, overview numbers, so our representations all live in Rn. Spatial rotations I Even though SO(3) is a three-dimensional space, it Preview 3 Axis-angle has the topology of projective three space P , which Rodrigues’s formula 3 Rotation matrices cannot be smoothly mapped to . Matrix exponential R Euler angles I And, we have lots of different applications, with different requirements: communication, operating on things, composition, interpolation, etc. Lecture 7 Choices Nonholonomic constraint II Representing Rotation I

Nonholonomic I Axis-angle constraint II Frobenius’s theorem

I Good for communication, geometrical insight Kinematic representation: I Rotation matrices goals, overview

I Good for operating on stuff, composition, analytical Spatial rotations Preview insight Axis-angle Rodrigues’s formula I Matrix exponential Rotation matrices Matrix exponential I Unit quaternions (aka Euler parameters) Euler angles

I Good for composition, analytical insight, sampling I Euler angles

I Good for communication, geometrical insight Lecture 7 Axis-angle Nonholonomic constraint II Representing Rotation I Euler’s theorem: every spatial rotation has a rotation axis. Nonholonomic n constraint II I Let O, nˆ, θ, be ... Frobenius’s theorem Kinematic I Let rot(nˆ, θ) be the corresponding representation: rotation. goals, overview Spatial rotations I Many to one: Preview Axis-angle Rodrigues’s formula I rot(−nˆ, −θ) = rot(nˆ, θ) Rotation matrices I rot(nˆ, θ + 2kπ) = rot(nˆ, θ), for any integer k. Matrix exponential Euler angles I So, restrict θ to [0, π]. But not smooth at the edges. I When θ = 0, the rotation axis is indeterminate, giving an inﬁnity-to-one mapping. I Again you can ﬁx by adopting a convention for nˆ, but result is not smooth. I (Or, what about using the product, θnˆ? Later.) Lecture 7 What do we want from axis-angle? Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Frobenius’s theorem I Operate on points Kinematic I Rodrigues’s formula representation: goals, overview

I Compose rotations, average, interpolate, sampling, Spatial rotations ...? Preview Axis-angle I Not using axis-angle Rodrigues’s formula Rotation matrices Matrix exponential I Convert to other representations? There aren’t any Euler angles yet. But, later we will use axis-angle big time. It’s very close to quaternions. Lecture 7 Rodrigues’s formula Nonholonomic constraint II Representing Others derive Rodrigues’s formula using rotation Rotation I matrices: ugly and messy. The geometrical approach is Nonholonomic clean and insightful. constraint II Frobenius’s theorem

I Given point x, decompose into Kinematic representation: components parallel and goals, overview

perpendicular to the rotation axis Spatial rotations Preview Axis-angle n x = nˆ(nˆ · x) − nˆ × (nˆ × x) n n x Rodrigues’s formula Rotation matrices nx Matrix exponential Euler angles I Only x⊥ is affected by the rotation, x

yielding Rodrigues’s formula: n n x x x0 = nˆ(nˆ·x)+sin θ (nˆ×x)−cos θ nˆ×(nˆ×x) O

I A common variation:

x0 = x+(sin θ) nˆ×x+(1−cos θ) nˆ×(nˆ×x) Lecture 7 Rotation matrices Nonholonomic constraint II Representing I Choose O on rotation axis. Choose frame Rotation I (uˆ1, uˆ2, uˆ3). 0 0 0 I Let (uˆ , uˆ , uˆ ) be the image of that frame. Nonholonomic 1 2 3 constraint II ˆ0 ˆ Frobenius’s theorem I Write the ui vectors in ui coordinates, and collect Kinematic them in a matrix: representation: goals, overview    ˆ ˆ0  a11 u1 · u1 Spatial rotations ˆ0 ˆ ˆ0 Preview u1 =  a21  =  u2 · u1  Axis-angle ˆ ˆ0 Rodrigues’s formula a31 u3 · u1 Rotation matrices Matrix exponential 0    ˆ ˆ  Euler angles a12 u1 · u2 ˆ0 ˆ ˆ0 u2 =  a22  =  u2 · u2  ˆ ˆ0 a32 u3 · u2    ˆ ˆ0  a13 u1 · u3 ˆ0 ˆ ˆ0 u3 =  a23  =  u2 · u3  ˆ ˆ0 a33 u3 · u3 0 0 0 A = aij = uˆ1|uˆ2|uˆ3 Lecture 7 So many numbers! Nonholonomic constraint II Representing Rotation I I A rotation matrix has nine numbers, Nonholonomic I but spatial rotations have only three degrees of constraint II freedom, Frobenius’s theorem Kinematic I leaving six excess numbers ... representation: goals, overview

I There are six constraints that hold among the nine Spatial rotations Preview numbers. Axis-angle Rodrigues’s formula 0 0 0 Rotation matrices |uˆ | = |uˆ | = |uˆ | = 1 Matrix exponential 1 2 3 Euler angles 0 0 0 uˆ3 = uˆ1 × uˆ2 ˆ0 I i.e. the ui are unit vectors forming a right-handed coordinate system.

I Such matrices are called orthonormal or rotation matrices. Lecture 7 Rotating a point Nonholonomic constraint II Representing Rotation I

I Let (x1, x2, x3) be coordinates of x in frame Nonholonomic constraint II (uˆ1, uˆ2, uˆ3). Frobenius’s theorem 0 Kinematic I Then x is given by the same coordinates taken in representation: ˆ0 ˆ0 ˆ0 goals, overview the (u1, u2, u3) frame: Spatial rotations 0 0 0 0 Preview x =x1uˆ1 + x2uˆ2 + x3uˆ3 Axis-angle Rodrigues’s formula ˆ ˆ ˆ Rotation matrices =x1Au1 + x2Au2 + x3Au3 Matrix exponential Euler angles =A(x1uˆ1 + x2uˆ2 + x3uˆ3) =Ax

I So rotating a point is implemented by ordinary matrix multiplication. Lecture 7 Sub- and superscript notation for rotating a Nonholonomic constraint II Representing point Rotation I

Nonholonomic constraint II Frobenius’s theorem

I Let A and B be coordinate frames. Kinematic A representation: I Let x be coordinates in frame A. goals, overview B Spatial rotations I Let R be the rotation matrix that rotates frame B to A Preview frame A. Axis-angle Rodrigues’s formula B Rotation matrices I Then (see previous slide) AR represents the rotation Matrix exponential of the point x: Euler angles B 0 B B x = AR x

I Note presuperscripts all match. Both points, and xform, must be written in same coordinate frame. Lecture 7 Coordinate transform Nonholonomic constraint II Representing Rotation I

B Nonholonomic There is another use for AR: constraint II A B Frobenius’s theorem I x and x represent the same point, in frames A and Kinematic B resp. representation: goals, overview I To transform from A to B: Spatial rotations Preview Axis-angle B B A x = R x Rodrigues’s formula A Rotation matrices Matrix exponential Euler angles I For coord xform, matrix subscript and vector superscript “cancel”. Rotation from B to A is the same as coordinate transform from A to B. Lecture 7 Example rotation matrix Nonholonomic constraint II Representing Rotation I

Nonholonomic BR = Bx By Bz constraint II A A A A Frobenius’s theorem   1 0 0 Kinematic representation: =  0 0 −1  goals, overview 0 1 0 Spatial rotations Preview Axis-angle B Rodrigues’s formula How to remember what AR does? Pick a Rotation matrices Matrix exponential coordinate axis and see. The x axis isn’t Euler angles very interesting, so try y:

 1 0 0   0   0   0 0 −1   1  =  0  0 1 0 0 1 Lecture 7 Nice things about rotation matrices Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Frobenius’s theorem I Composition of rotations: {R1; R2} = R2R1. Kinematic representation: ({x; y} means do x then do y.) goals, overview

I Inverse of rotation matrix is its transpose Spatial rotations Preview B −1 A B T R = R = R . Axis-angle A B A Rodrigues’s formula Rotation matrices I Coordinate xform of a rotation matrix: Matrix exponential Euler angles B B A A R = AR R BR Lecture 7 Converting rot(nˆ, θ) to R Nonholonomic constraint II Representing Rotation I

I Ugly way: deﬁne frame with zˆ aligned with nˆ, use Nonholonomic coordinate xform of previous slide. constraint II Frobenius’s theorem

I Keen way: Rodrigues’s formula! Kinematic representation: goals, overview 0 x = x + (sin θ) nˆ × x + (1 − cos θ) nˆ × (nˆ × x) Spatial rotations Preview Axis-angle I Deﬁne “cross product matrix” N: Rodrigues’s formula Rotation matrices Matrix exponential   Euler angles 0 −n3 n2 N =  n3 0 −n1  −n2 n1 0

so that Nx = nˆ × x Lecture 7 ... using Rodrigues’s formula ... Nonholonomic constraint II Representing Rotation I I Substituting the cross product matrix N into

Rodrigues’s formula: Nonholonomic constraint II x0 = x + (sin θ)Nx + (1 − cos θ)N2x Frobenius’s theorem Kinematic representation: goals, overview I Factoring out x Spatial rotations Preview 2 Axis-angle R = I + (sin θ)N + (1 − cos θ)N Rodrigues’s formula Rotation matrices Matrix exponential I That’s it! Rodrigues’s formula in matrix form. If you Euler angles want to you could expand it:

 2 2  n1 + (1 − n1)cθ n1n2(1 − cθ) − n3sθ n1n3(1 − cθ) + n2sθ 2 2  n1n2(1 − cθ) + n3sθ n2 + (1 − n2)cθ n2n3(1 − cθ) − n1sθ  2 2 n1n3(1 − cθ) − n2sθ n2n3(1 − cθ) + n1sθ n3 + (1 − n3)cθ where cθ = cos θ and sθ = sin θ. Ugly. Lecture 7 Rodrigues’s formula for differential rotations Nonholonomic constraint II Representing Rotation I Consider Rodrigues’s formula for a differential rotation rot(nˆ, dθ). Nonholonomic constraint II Frobenius’s theorem 0 2 x =(I + sin dθN + (1 − cos dθ)N )x Kinematic representation: =(I + dθN)x goals, overview Spatial rotations Preview so Axis-angle Rodrigues’s formula Rotation matrices dx =Nx dθ Matrix exponential Euler angles =nˆ × x dθ

It follows easily that differential rotations are vectors: you can scale them and add them up. We adopt the convention of representing angular velocity by the unit vector nˆ times the angular velocity. Lecture 7 Converting from R to rot(nˆ, θ) ... Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Frobenius’s theorem

Kinematic representation: goals, overview

I Problem: nˆ isn’t deﬁned for θ = 0. Spatial rotations Preview I We will do it indirectly. Convert R to a unit quaternion Axis-angle Rodrigues’s formula (next lecture), then to axis-angle. Rotation matrices Matrix exponential Euler angles Lecture 7 Matrix exponential Nonholonomic constraint II Representing Recall deﬁnition of real exponential Rotation I

∞ X 1 Nonholonomic ex = xk constraint II k! Frobenius’s theorem k=0 Kinematic representation: To deﬁne matrix exponential, just plug in a matrix. Let’s goals, overview Spatial rotations try the cross product matrix times the angle: θN: Preview Axis-angle Rodrigues’s formula θN 1 2 2 1 3 3 Rotation matrices e = I + θN + θ N + θ N + ... Matrix exponential 2! 3! Euler angles Note that N3 = −N, N4 = −N2, etc. 1 1 1 1 eθN = I + N(θ − θ3 + θ5 ...) + N2( θ2 − θ4 ...) 3! 5! 2! 4! Use series expansions of sin and cos to get Rodriguez’s formula! Lecture 7 Euler angles Nonholonomic constraint II Representing Rotation I I Three numbers to describe spatial rotations. ZYZ convention: Nonholonomic constraint II 00 0 (α, β, γ) 7→ rot(γ, zˆ ) rot(β, yˆ ) rot(α, zˆ) Frobenius’s theorem z Kinematic representation: I Can we represent an arbitrary goals, overview rotation? z Spatial rotations Preview Rotate α about zˆ until Axis-angle Rodrigues’s formula y 0 000 yˆ ⊥ zˆ ; Rotation matrices Matrix exponential 0 Rotate β about yˆ until Euler angles zˆ00 k zˆ000; x Rotate γ about zˆ00 until y yˆ00 = yˆ000.

0 I Note two choices for yˆ ... x

I . . . except sometimes inﬁnite choices. Lecture 7 Converting from Euler angles to rotation Nonholonomic constraint II Representing matrices Rotation I

Nonholonomic notation constraint II Frobenius’s theorem { }, { }, { }, { } Kinematic I Deﬁne frames 0 1 2 3 so that representation: goals, overview I rot(α, zˆ) maps {0} to {1}, etc., Spatial rotations i I As before R is the rotation matrix rotating frame {i} Preview j Axis-angle to frame {j}, written in frame {i} coordinates. Rodrigues’s formula Rotation matrices k i Matrix exponential I Let (j R) be the same matrix, written in frame {k} Euler angles coordinates.

I Then the correct sequence, written in a common coordinate frame, would be

0 0 2 0 1 0 0 3R = (3R) (2R) (1R) Lecture 7 Moving frame versus ﬁxed frame Nonholonomic constraint II Representing Rotation I

Switch to moving frame Nonholonomic constraint II Frobenius’s theorem

I Use the coordinate transform of a matrix formula: Kinematic representation: goals, overview 0R = 0(2R) 0(1R) 0R 3 3 2 1 Spatial rotations 0 2 2 0 1 1 0 Preview = (2R 3R 0R)(1R 2R 0R) 1R Axis-angle Rodrigues’s formula 0 1 2 = R R R Rotation matrices 1 2 3 Matrix exponential Euler angles I Wow! You can switch between moving frame and ﬁxed frame, if you also switch the order!

I You could also have derived the above, just by 0 interpreting 3R as a coordinate transform. Lecture 7 From (α, β, γ) to R Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Frobenius’s theorem

0 0 1 2 Kinematic R = R R R representation: 3 1 2 3 goals, overview       cα −sα 0 cβ 0 sβ cγ −sγ 0 Spatial rotations Preview =  sα cα 0   0 1 0   sγ cγ 0  Axis-angle Rodrigues’s formula 0 0 1 −sβ 0 cβ 0 0 1 Rotation matrices Matrix exponential  cα cβ cγ − sα sγ −cα cβ sγ − sα cγ cα sβ  Euler angles =  sα cβ cγ + cα sγ −sα cβ sγ + cα cγ sα sβ  −sβ cγ sβ sγ cβ Lecture 7 From R to (α, β, γ) the ugly way Nonholonomic constraint II Representing Rotation I I Case 1: r33 = 1, β = 0. α − γ is indeterminate.

  Nonholonomic cos(α + γ) − sin(α + γ) 0 constraint II R =  sin(α + γ) cos(α + γ) 0  Frobenius’s theorem Kinematic 0 0 1 representation: goals, overview

I Case 2: r = −1, β = πor − π. α + γ is Spatial rotations 33 Preview indeterminate. Axis-angle Rodrigues’s formula Rotation matrices  − (α − γ) − (α − γ)  Matrix exponential cos sin 0 Euler angles R =  − sin(α − γ) cos(α − γ) 0  0 0 −1

I For generic case: solve 3rd column for β. (Sign is free choice.) Solve third column for α and third row for γ.

I ... but there are numerical issues ... Lecture 7 From R to (α, β, γ) the clean way Nonholonomic constraint II Representing I Let Rotation I

σ = α + γ Nonholonomic constraint II δ = α − γ Frobenius’s theorem Kinematic representation: I Then goals, overview Spatial rotations Preview r22 + r11 = cos σ(1 + cos β) Axis-angle Rodrigues’s formula Rotation matrices r22 − r11 = cos δ(1 − cos β) Matrix exponential Euler angles r21 + r12 = sin δ(1 − cos β)

r21 − r12 = sin σ(1 + cos β)

(No special cases for cos β = ±1?)

I Solve for σ and δ, then for α and γ, then ﬁnally

−1 β = tan (r13 cos α + r23 sin α, r33) Lecture 7 But what about sin β = 0? Nonholonomic constraint II Representing Rotation I

Nonholonomic constraint II Frobenius’s theorem

Kinematic representation: I How can this method work without explicitly goals, overview

addressing the singularity? Spatial rotations Preview I When β = 0, σ is determined and δ is not. When Axis-angle Rodrigues’s formula β = π, δ is determined and σ is not. Rotation matrices Matrix exponential −1 Euler angles I If your tan handles (0, 0), you can just let it go!