The Lie group SE(3) - Penn Engineeringmeam620/slides/kinematicsI.pdf · Rigid Body Kinematics...

Rigid Body Kinematics

University of Pennsylvania 1

The Lie group SE(3)

( )⎭⎬⎫

⎩⎨⎧

===∈∈⎥⎦

⎤⎢⎣

⎡== ×

×

1,,,,1

3 333

31

RIRRRRrR0

rRAA TTRRSE

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



Homogeneous Transformation MatrixCoordinate transformation from {B} to {A}

Homogeneous transformation matrix

OAPBB

APA ′′ += rrRr

x

y

z

ArP

O

BrP

ArO’

z'

y'

x'

{A}

O'A

BP

⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡

×

′

111 31

PBOAB

APA r0

rRr

⎥⎦

⎤⎢⎣

⎡=

× 131

'

0rRA

OAB

A

BA

.131

13 ⎥⎦

⎤⎢⎣

⎡=

×

×

00RA B

A

BA

.131

33 ⎥⎦

⎤⎢⎣

⎡=

×

′×

0rIA

OAB

A

Special cases

1.

2.



Homogeneous CoordinatesDescription of a point

(x, y, z)Description of a plane

(t, u, v, s)

Equation of a circlex2 + y2 + z2 = a2

Homogeneous coordinatesDescription of a point

(x, y, z, w)Equation of a plane

tx + uy + vz + sw = 0Equation of a sphere

x2 + y2 + z2 = a2 w2

tx + uy + vz + s = 0

(t, u, v)

Central ideasEquivalence classProjective space P3, and not Euclidean space R3

Mathematical and practical advantages



Example: TranslationTranslation along the z-axis through h

( )⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

1000100

00100001

,h

hzTrans

x

y

z

y'

z'⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

1'''

1000100

00100001

1zyx

hzyx

x

y

z

h

Transformation Displacement

h



Example: RotationRotation about the x-axis through θ

( )⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

θθθ−θ

=θ

10000cossin00sincos00001

,xRot

x

y

z

θ

y'z'

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

θθθ−θ

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

1'''

10000cossin00sincos00001

1zyx

zyx

x

y

zθ

Transformation Displacement



Example: RotationRotation about the y-axis through θ

y

z

x

θ

z'x'

( )⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

θθ−

θθ

=θ

10000cos0sin00100sin0cos

,yRot

Rotation about the z-axis through θ

( )⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡θθθ−θ

=θ

1000010000cossin00sincos

,zRot

y

z

x

θ

y'

x'



Mobile Robots

x

y

θ

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡θθθ−θ

=100

cossinsincos

yx

gWRW

R

WAR



Example: Displacement of PointsDisplace (7, 3, 2) through a sequence of:1. Rot(z, 90)2. Rot(y, 90) 3. Trans(4, -3, 7) in the frame F: (x, y, z):

Trans(4, -3, 7) Rot(y, 90) Rot(z, 90)

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

1000010000010010

1000000100100100

100071003010

4001

P1

P2

(6, 4, 10, 1)P3

x

y

z

P0 (7, 3, 2, 1)



Example: Transformation of PointsSuccessive transformations of (7, 3, 2):1. Trans(4, -3, 7) 2. Rot(y, 90) 3. Rot(z, 90)in a body fixed frame

Trans(4, -3, 7) Rot(y, 90) Rot(z, 90)x'

y'

z'

x’’

y’’ (7, 3, 2, 1)

(6, 4, 10, 1)

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

1000010000010010

1000000100100100

100071003010

4001

x

y

z

(7, 3, 2, 1)



SE(3) is a Lie groupSE(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.For every element A ∈ SE(3), there is an identity inverse, A-1 ∈ SE(3), such that A A-1 = I.

SE(3) is a continuous group.the binary operation above is a continuous operation ⎯ the product of any two elements in SE(3) is a continuous function of the two elementsthe 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)].



Composition of DisplacementsDisplacement from {A} to {B}

Displacement from {B} to {C}

Displacement from {A} to {C}x

y

z

O

z'

y'

x'

{A}

O'

z''

x''y''

O''

{B}

POSITION 1

POSITION 2

POSITION 3

{C}

,131

'

⎥⎦

⎤⎢⎣

⎡=

×0rRA

OAB

A

BA

,131

⎥⎦

⎤⎢⎣

⎡=

×

′′

0rRA

OBC

B

CB

⎥⎦

⎤⎢⎣

⎡ +××=

⎥⎦

⎤⎢⎣

⎡×⎥

⎦

⎤⎢⎣

⎡=

⎥⎦

⎤⎢⎣

⎡=

′′′

′′′

′′

1

11

1

0rrRRR

0rR

0rR

0rRA

OAOBB

AC

BB

A

OBC

BOAB

A

OAC

A

CA

Note XAY describes the displacement of the body-fixed frame from {X} to {Y} in reference frame {X}



Composition (continued)

x

y

z

O

z'

y'

x'

{A}

O'

z''

x''y''

O''

{B}

POSITION 1

POSITION 2

POSITION 3

{C}

Note XAY describes the displacement of the body-fixed frame from {X} to {Y} in reference frame {X}

Composition of displacementsDisplacements are generally described in a body-fixed frameExample: BAC is the displacement of a rigid body from B to C relative to theaxes of the “first frame” B.

Composition of transformationsSame basic ideaAAC = AAB

BAC

Note that our description of transformations (e.g., BAC) is relativeto the “first frame” (B, the frame with the leading superscript).



Transformations associated with the Lie group SE(3)



Differentiable Manifold

DefinitionA manifold of dimension n is a set M which is locally homeomorphic* to Rn.

R2

dim 2manifold

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

Smooth mapA map f from U ⊂ Rm to V ⊂ Rn is smooth if all partial derivatives of f, of all orders, exist and are continuous.

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



Examples

[ )

0 1

( ) 21)(,1,1:

xxxfRf

−=→−

P



Smooth Manifold

Differentiable manifold is locally homeomorphic to Rn

Parametrize the manifold using a set of local coordinate charts(U, φ ), (V, Ψ ), …

Require compatibility on overlapsC∞ related

Collection of charts covering M

Rn

dim nmanifold

φ

U V

Ψ

Rn



Actions of SE(3)M any smooth manifold

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

Φ(I, x) = x, x∈ MΦ(A, Φ(B, x) ) = Φ(AB, x), A, B ∈ SE(3), x ∈ M



Actions of SE(3)

1. Action of SE(3) on R3

Displacement of points, p → Ap

2. Action of SE(3) on itselfΦ: SE(3) × SE(3) → SE(3) ΦQ: A → Q A Q-1

A denotes a displacement of a rigid bodyΦQ transforms the displacement A

Actions on the Lie algebra (later)



What happens when you want to describe the displacement of the body-fixed frame from {A} to {B} in reference frame {F}?

Displacement is described in {A} by the homogeneous transform, AAB.

Want to describe the same displacement in {F}. The position and orientation of {A} relative to {F} is given by the homogeneous transform, FAA.

The same displacement which moves a body-fixed frame from {A} to {B}, will move another body-fixed frame from {F} to {G}:

FAG = FAA AAB

BAG

x

y

z

O

z'

y'

x'

{A} O'

{B}

POSITION 1

POSITION 2

{F}

{G}

X

Y

Z

X’

Y’

Z’

D

D

T

T

FAG = FAA AAB (FAA)-1







FAG = FAA AAB

BAG

x

y

z

O

z'

y'

x'

{A} O'

{B}

POSITION 1

POSITION 2

{F}

{G}

X

Y

Z

X’

Y’

Z’

D

D

T

T








FAG = FAA AAB

BAG


x

y

z

O

z'

y'

x'

{A} O'

{B}

POSITION 1

POSITION 2

{F}

{G}

X

Y

Z

X’

Y’

Z’

D

D

T

T



Rotational motion in R3

Specialize to SO(3)



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.

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

Later: Chasles’ Theorem for Rotations



Proof of Euler’s Theorem for Spherical Displacements

Displacement from {F} to {M}

P = Rp

Solve the eigenvalue problem (find the vector that is unaffected by R):

Rp = λ p

φφ

v2Rv1

v1

Rv2

u

axis of rotation

eigenspace spanned by x and x

_

( )( )

( ) ( ) 0][

2112221131133311

23323322

33221123

=−+−+−λ−

++λ+λ−

RRRRRRRRRRRR

RRR( ) ( )( ) 0111 332211

2 =+−++λ−λ−λ RRR

0=λ− IR

xp

xp

up

==λ

==λ

==λ

φ−

φ

23

22

11

,

,

,1

i

i

e

e

Three eigenvalues and eigenvectors are:

( )121cos 332211 −++=φ RRR

where



The Axis/Angle for a Rotation Matrix

φφ

v2Rv1

v1

Rv2

u

axis of rotation


_

( ) ( )

( ) ( )2121

21

21,

21

2,

21

vvxvvx

xxvxxv

ii

i

+=−=

−=+=

( ) ( )

( )φ+φ=

+=

λ+λ=+=

φ−φ

sincos21

21

21

21

321

vv

xx

xxxRRxRv

ii ee

( ) ( )

( )φ+φ−=

−=

λ−λ=−=

φ−φ

cossin2

22

21

322

vv

xx

xxxRRxRv

ii eei

ii

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡φφφ−φ

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

1000cossin0sincos

2121 uvvuvvR

Q ΛQ



R= Q Λ QT

Define

and look at the displacement in the new reference frame, {F’}.

φφ

v2Rv1

v1

Rv2

u

axis of rotation


_

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡==′ uvvQR 21F

F


[ ]TFF

MF

FF

MF RRRR ′′

′′ =

QT QRΛ

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡φφφ−φ

=Λ1000cossin0sincos

Recall

Any rotation is a left action of SO(3) on a canonical (?) element, Λ.

The Lie group SE(3) - Penn Engineeringmeam620/slides/kinematicsI.pdf · Rigid Body Kinematics...

Documents

Transcript of The Lie group SE(3) - Penn Engineeringmeam620/slides/kinematicsI.pdf · Rigid Body Kinematics...