The Lie Group SE(3)

The Lie group SE(3)

⎧ ⎡ R r⎤ 3×3 3 T T ⎫ SE()3 = ⎨A A = ⎢ ⎥, R ∈ R , r ∈ R , R R = RR = I, R =1⎬ ⎩ ⎣01×3 1⎦ ⎭

http://www.seas.upenn.edu/~meam620/notes/RigidBodyMotion3.pdf

University of Pennsylvania 1 Rigid Body Kinematics Homogeneous Transformation Matrix z' Coordinate transformation from {B} to {A}

z A P′ A B P A O′ P r = R B r + r {A} B BrP y'

ArP O' A ArO’ y ⎡ ArP ⎤ ⎡ AR ArO′ ⎤⎡ B rP ⎤ O ⎢ ⎥=⎢ B ⎥⎢ ⎥ ⎣ 1 ⎦ ⎣ 01×3 1 ⎦⎣ 1 ⎦ x' Special cases x A A ⎡ RB 03×1⎤ Homogeneous transformation matrix AB = ⎢ ⎥. 1. 0 1 ⎡ A A O' ⎤ ⎣ 1×3 ⎦ A RB r A O′ AB = ⎢ ⎥ ⎡I r ⎤ 2. A A = 3×3 . ⎣ 01×3 1 ⎦ B ⎢ ⎥ ⎣01×3 1 ⎦

University of Pennsylvania 2 Rigid Body Kinematics Homogeneous Coordinates

z Description of a point (x, y, z) z Description of a plane (t, u, v) (t, u, v, s)

z Equation of a circle x2 + y2 + z2 = a2

Homogeneous coordinates tx + uy + vz + s = 0 z Description of a point (x, y, z, w) z Equation of a plane Central ideas tx + uy + vz + sw = 0 z Equivalence class z Equation of a sphere z Projective space P3, x2 + y2 + z2 = a2 w2 and not Euclidean space R3 Mathematical and practical advantages

University of Pennsylvania 3 Rigid Body Kinematics Example: Translation z Translation along the z-axis through h ⎡1 0 0 0⎤ ⎡x⎤ ⎡1 0 0 0⎤⎡x'⎤ ⎢0 1 0 0⎥ ⎢y⎥ ⎢0 1 0 0⎥⎢y'⎥ Trans()z,h = ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥⎢ ⎥ ⎢0 0 1 h⎥ ⎢z⎥ ⎢0 0 1 h⎥⎢z'⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣0 0 0 1⎦ ⎣1⎦ ⎣0 0 0 1⎦⎣1 ⎦ z' z z h y' h y y

Transformation Displacement x x University of Pennsylvania 4 Rigid Body Kinematics Example: Rotation z Rotation about the x-axis through θ ⎡1 0 0 0⎤ ⎡x⎤ ⎡1 0 0 0⎤⎡x'⎤ ⎢0 cosθ − sin θ 0⎥ ⎢y⎥ ⎢0 cosθ − sin θ 0⎥⎢y'⎥ Rot()x,θ = ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥⎢ ⎥ ⎢0 sin θ cosθ 0⎥ ⎢z⎥ ⎢0 sin θ cosθ 0⎥⎢z'⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣0 0 0 1⎦ ⎣1⎦ ⎣0 0 0 1⎦⎣1 ⎦

z z z' θ y'

θ y y

Transformation Displacement x x University of Pennsylvania 5 Rigid Body Kinematics Example: Rotation

Rotation about the y-axis through θ Rotation about the z-axis through θ ⎡ cosθ 0 sin θ 0⎤ ⎡cosθ − sin θ 0 0⎤ ⎢ 0 1 0 0⎥ ⎢sin θ cosθ 0 0⎥ Rot()y,θ = ⎢ ⎥ Rot()z,θ = ⎢ ⎥ ⎢− sin θ 0 cosθ 0⎥ ⎢ 0 0 1 0⎥ ⎢ ⎥ ⎢ ⎥ ⎣ 0 0 0 1⎦ ⎣ 0 0 0 1⎦ x x' x z' x' θ θ z z

y y y' University of Pennsylvania 6 Rigid Body Kinematics Mobile Robots

y R θ

x ⎡cosθ − sin θ x⎤ W g = ⎢sin θ cosθ y⎥ WR ⎢ ⎥ W ⎣⎢ 0 0 1⎦⎥ AR University of Pennsylvania 7 Rigid Body Kinematics Example: Displacement of Points

Displace (7, 3, 2) through a sequence of: 1. Rot(z, 90) z 2. Rot(y, 90) (6, 4, 10, 1) 3. Trans(4, -3, 7) P3 in the frame F: (x, y, z): P1

y Trans(4, -3, 7) Rot(y, 90) Rot(z, 90) 1 0 0 4 0 0 1 0 0 −1 0 0 ⎡ ⎤⎡ ⎤⎡ ⎤ P2 ⎢0 1 0 − 3⎥⎢ 0 1 0 0⎥⎢1 0 0 0⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ x ⎢0 0 1 7 ⎥⎢−1 0 0 0⎥⎢0 0 1 0⎥ P0 (7, 3, 2, 1) ⎢ ⎥⎢ ⎥⎢ ⎥ ⎣0 0 0 1 ⎦⎣ 0 0 0 1⎦⎣0 0 0 1⎦

University of Pennsylvania 8 Rigid Body Kinematics Example: Transformation of Points

Successive transformations of (7, 3, 2): 1. Trans(4, -3, 7) y’’ 2. Rot(y, 90) (7, 3, 2, 1) 3. Rot(z, 90) in a body fixed frame x’’ y'

z' (6, 4, 10, 1) Trans(4, -3, 7) Rot(y, 90) Rot(z, 90) z x' ⎡1 0 0 4 ⎤⎡ 0 0 1 0⎤⎡0 −1 0 0⎤ ⎢0 1 0 − 3⎥⎢ 0 1 0 0⎥⎢1 0 0 0⎥ ⎢ ⎥⎢ ⎥⎢ ⎥ y ⎢0 0 1 7 ⎥⎢−1 0 0 0⎥⎢0 0 1 0⎥ ⎢0 0 0 1 ⎥⎢ 0 0 0 1⎥⎢0 0 0 1⎥ (7, 3, 2, 1) ⎣ ⎦⎣ ⎦⎣ ⎦ x

University of Pennsylvania 9 Rigid Body Kinematics SE(3) is a Lie group

SE(3) satisfies the four axioms that must be satisfied by the elements of an algebraic group:

The set is closed under the binary operation. In other words, if A and B are any two matrices in SE(3), AB ∈ SE(3).

The binary operation is associative. In other words, if A, B, and C are any three matrices ∈ SE(3), then (AB) C = A (BC).

For every element A ∈ SE(3), there is an identity element given by the 4×4 identity matrix, I∈ SE(3), such that AI = A. -1 -1 For every element A ∈ SE(3), there is an identity inverse, A ∈ SE(3), such that A A = I. SE(3) is a continuous group. z the binary operation above is a continuous operation ⎯ the product of any two elements in SE(3) is a continuous function of the two elements z the inverse of any element in SE(3) is a continuous function of that element. In other words, SE(3) is a differentiable manifold. A group that is a differentiable manifold is called a Lie group[ Sophus Lie (1842-1899)].

University of Pennsylvania 10 Rigid Body Kinematics Composition of Displacements

z' Displacement from {A} to {B} POSITION 2 A A O' z {B} A ⎡ R B r ⎤ AB = ⎢ ⎥, 0 1 {A} ⎣ 1×3 ⎦ y' Displacement from {B} to {C} O' POSITION 1 B B O′′ B ⎡ RC r ⎤ A = , y {C} z'' C ⎢ ⎥ O ⎣ 01×3 1 ⎦ O'' Displacement from {A} to {C} x' x ⎡ A R ArO′′ ⎤ A A = C x'' C ⎢ ⎥ y'' ⎣ 0 1 ⎦ POSITION 3 A A O′ B B O′′ ⎡ R B r ⎤ ⎡ RC r ⎤ = ⎢ ⎥ × ⎢ ⎥ Note XA describes the ⎣ 0 1 ⎦ ⎣ 0 1 ⎦ Y A B A B O′′ A O′ displacement of the body-fixed ⎡ R B × RC R B × r + r ⎤ = ⎢ ⎥ frame from {X} to {Y} in ⎣ 0 1 ⎦ reference frame {X}

University of Pennsylvania 11 Rigid Body Kinematics Composition (continued) z' Composition of displacements POSITION 2 {B} z Displacements are generally described z in a body-fixed frame {A} y' z B Example: AC is the displacement of a O' rigid body from B to C relative to the POSITION 1 axes of the “first frame” B. y {C} z'' O

O'' Composition of transformations x'

z Same basic idea x A A B z AC = AB AC x'' y'' POSITION 3 Note that our description of B X transformations (e.g., AC) is relative Note AY describes the to the “first frame” (B, the frame with displacement of the body-fixed the leading superscript). frame from {X} to {Y} in reference frame {X}

University of Pennsylvania 12 Rigid Body Kinematics

Transformations associated with the Lie group SE(3)

University of Pennsylvania 13 Rigid Body Kinematics Differentiable Manifold

Definition A manifold of dimension n is a set M which is locally n homeomorphic* to R .

Homeomorphism: A map f from M to N and its inverse, f-1 are both continuous.

Smooth map A map f from U ⊂ Rm to V ⊂ Rn is smooth if all partial derivatives of f, of all orders, exist and are continuous. dim 2 manifold Diffeomorphism A smooth map f from U ⊂ Rn to V ⊂ Rn is a diffeomorphism if all partial derivatives of f-1, of all orders, exist and are continuous.

University of Pennsylvania 14 Rigid Body Kinematics Examples

x f : ()−1,1 → R, f (x) = 1− x2

[) P 01

University of Pennsylvania 15 Rigid Body Kinematics Smooth Manifold z Differentiable manifold is locally homeomorphic to Rn z Parametrize the manifold using a set of local coordinate charts (U, φ ), (V, Ψ ), … Ψ z Require compatibility on overlaps C∞ related U V Rn z Collection of charts covering M φ dim n manifold

University of Pennsylvania 16 Rigid Body Kinematics Actions of SE(3) M any smooth manifold z Think R3, SE(3), subgroups of SE(3)

A left action of SE(3) on M is a smooth map, Φ: SE(3) × M → M, such that z Φ(I, x) = x, x∈ M z Φ(A, Φ(B, x) ) = Φ(AB, x), A, B ∈ SE(3), x ∈ M

University of Pennsylvania 17 Rigid Body Kinematics Actions of SE(3)

1. Action of SE(3) on R3 z Displacement of points, p → Ap

2. Action of SE(3) on itself Φ: SE(3) × SE(3) → SE(3) -1 ΦQ: A → QAQ z A denotes a displacement of a rigid body

z ΦQ transforms the displacement A

Actions on the Lie algebra (later)

University of Pennsylvania 18 Rigid Body Kinematics What happens when you want to describe the displacement of the body- fixed frame from {A} to {B} in reference frame {F}?

z' D Displacement is described in {A} by the POSITION 2 {B} A homogeneous transform, AB.

y' z Want to describe the same displacement O' Z’ {A} in {F}. The position and orientation of {G} {A} relative to {F} is given by the T F POSITION 1 homogeneous transform, AA. x' D y O The same displacement which moves a Y’ body-fixed frame from {A} to {B}, will X’ move another body-fixed frame from x T Z {F} to {G}: F F A B AG = AA AB AG X {F} F F A F -1 AG = AA AB ( AA) Y

University of Pennsylvania 19 Rigid Body Kinematics What happens when you want to describe the displacement of the body- fixed frame from {A} to {B} in reference frame {F}?

z' D Displacement is described in {A} by the POSITION 2 {B} A homogeneous transform, AB.

University of Pennsylvania 20 Rigid Body Kinematics What happens when you want to describe the displacement of the body- fixed frame from {A} to {B} in reference frame {F}?

z' D Displacement is described in {A} by the POSITION 2 {B} A homogeneous transform, AB.

University of Pennsylvania 21 Rigid Body Kinematics

Rotational motion in R3

Specialize to SO(3)

University of Pennsylvania 22 Rigid Body Kinematics Euler’s Theorem for Rotations

Any displacement of a rigid body such that a point on the rigid body, say O, remains fixed, is equivalent to a rotation about a fixed axis through the point O.

Later: Chasles’ Theorem for Rotations

The most general rigid body displacement can be produced by a translation along a line followed (or preceded) by a rotation about that line.

University of Pennsylvania 23 Rigid Body Kinematics Proof of Euler’s Theorem for Spherical Displacements

Displacement from {F} to {M} axis of P = Rp rotation Solve the eigenvalue problem (find the vector that is unaffected by R): u eigenspace spanned_ by x and x Rp = λ p φ Rv R − λI = 0 φ 2

v2 v1 3 2 Rv1 − λ + λ ()R11 + R22 + R33 () (λ −1)(λ2 − λ(R + R + R −1)+1)= 0 − λ[ R22R33 − R32R23 + 11 22 33 ()() R11R33 − R13R31 + R11R22 − R12R21 ] = 0

Three eigenvalues and eigenvectors are:

λ1 =1, p1 = u where 1 λ = eiφ, p = x cosφ = ()R + R + R −1 2 2 2 11 22 33 −iφ λ3 = e , p2 = x

University of Pennsylvania 24 Rigid Body Kinematics The Axis/Angle for a Rotation Matrix

1 1 axis of Rv1 = ()Rx + Rx = ()λ2x + λ3x rotation 2 2 1 iφ −iφ u = ()e x + e x eigenspace spanned_ 2 by x and x = v1 cosφ + v2 sin φ φ Rv φ 2 i i Rv2 = ()Rx − Rx = ()λ2x − λ3x v 2 2 v 2 1 i iφ −iφ Rv1 = ()e x − e x 2

= −v1 sin φ + v2 cosφ 1 i v = ()x + x , v = ()x − x 1 2 2 2 ⎡ ⎤⎡ ⎤ ⎡ ⎤⎡cosφ − sin φ 0⎤ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ 1 1 R v1 v2 u = v1 v2 u sin φ cosφ 0 x = ()v − iv , x = ()v + iv ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ 2 1 2 2 1 2 ⎣⎢ ⎦⎥⎣⎢ ⎦⎥ ⎢⎣ ⎦⎥⎢⎣ 0 0 1⎦⎥ Q Q Λ University of Pennsylvania 25 Rigid Body Kinematics R= Q Λ QT

axis of rotation Define ⎡ ⎤ u eigenspace spanned_ by x and x F R = Q = ⎢v v u⎥ F′ ⎢ 1 2 ⎥ ⎣⎢ ⎦⎥ φ Rv φ 2 and look at the displacement in the new reference frame, {F’}. v2 v1 Rv1 F F A F -1 Recall AG = AA AB ( AA)

F′ F′ F F′ T RM ′ = R F RM [ R F ] Any rotation is a left action of Λ QT R Q SO(3) on a canonical (?) ⎡cosφ − sin φ 0⎤ element, Λ. Λ = ⎢sin φ cosφ 0⎥ ⎢ ⎥ ⎣⎢ 0 0 1⎦⎥ University of Pennsylvania 26