good4send

8/7/2019 good4send

1/10

Video Segment

Robotic Reconnaissance Team,University of Minnesota,ICRA 2000 video proceedings

Dynamics

Newton-Euler Formulation

Articulated Multi-Body Dynamics

Rigid Body Dynamics

Explicit Form

Recursive Algorithm

Lagrange Formulation

MA23

8/7/2019 good4send

2/10

( ) ( , ) ( )M q q V q q G q+ + =

Joint Space Dynamics

Centrifugal and Coriolis forces( , ) :V q q

( ) :G q Gravity forces

: Generalized forces

( ) :M q Mass Matrix - Kinetic Energy Matrix

:q Generalized Joint Coordinates

Kinetic Energy:

Potential Energy

Kii

Generalized Coordinates

q VM G + + =

FormulationsNewton-Euler Lagrange

-ni+1

-fi+1Link i-ni

-fi

Eliminate Internal Forces

ii

T

i

i

T

i

n Zf Z

= RST

..

revoluteprismatic

1

2

TK q qM=

Newton:Euler:

i ii C i i C iN I I = +

Cm F=v

Ni Fi

-ni

CiImi

n f

1

2

3

n f

-f-n

link 3n3

f3

-n1

-f1

-f2

-n2

n1

f1

link 1

-n3

f2

n2link 2

-f3

Link i

Pi+1

ni fi

-fi+1-ni+1

Ni Fi

CiImi

Recursive Equationsf F f

n N n F f

i i i

i i i C i i ii

= +

= + + + +

+ + +

1

1 1 1p p

ii i

i i

n Z

f Z=

RST

.

.

revolute

prismatic

Newton-Euler Algorithm

Velo

cities

, acce

l

and Inertialforces

forces

erations

8/7/2019 good4send

3/10

Newtons Law

F ma=

Fm

a

a particle

inertialFrame

v

d

dtmv F( ) =

Linear Momentum

rate of change of thelinear momentum is equalto the applied force

0

mv=

Angular Momentum

m Fv =take the moment /0

p v p = m F

d

dtm m m m( ) p v p v v v p v = + =

N

( )d

m Nd t

=p v

applied moment

angular momentum

=0

Fm

inertialFrame

mv

p

0

p mv=

Rigid Body

Rotational Motion

Angular Momentum

mi

v pi i=

= p vi i ii

m

= mi i ii

p p( )

m dvi

( : )density

ip

( )V

p p dv =

= z[ ]pp dvV

I=

Inertia Tensor

p p p p = ( ) ( )

( )V

p p dv =

I=

Euler Equation

= N

I I N + =

( )

d

I Nd t =

d

dtmv F( ) =

Newton Equation

Angular MomentumLinear Momentum

F=

mv=

ma F=

Inertia Tensor

I dvV

= z pp ( ) ( ) = pp p p ppT TI3I I dvT T

V

= z[( ) ]p p pp3 p =

L

N

MM

M

O

Q

PP

P

x

y

z

; p pT

x y z= + +2 2 2

( ) ( )p p

T

I x y z32 2 2

1 0 0

0 1 00 0 1

= + +

F

HGG

I

KJJ

ppTx

y

z

x y z

x xy xz

xy y yz

xz yz z

=

L

N

MMM

O

Q

PPP

=

L

N

MMM

O

Q

PPP

a f

2

2

2

y z xy xz

xy z x yz

xz yz x y

2 2

2 2

2 2

+

+

+

L

N

MMM

O

Q

PPP

( ) =pp

8/7/2019 good4send

4/10

I

I I I

I I I

I I I

xx xy xz

xy yy yz

xz yz zz

=

L

N

MMM

O

Q

PPP

I y z dxdydz

I z x dxdydz

I x y dxdydz

I xy dxdydz

I xz dxdydz

I yz dxdydz

xx

yy

zz

xy

xz

yz

= +

= +

= +

=

=

=

zzzzzzzzzzzzzzzzzz

( )

( )

( )

2 2

2 2

2 2

Moments ofInertia

Products ofInertia

Inertia Tensor Parallel Axis theorem

p

pc

{A}

x

y

z

c

c

c

RS|

T|

UV|

W|

3[( ) ]T T

A C C C C C I I m I = + p p p p2 2( )A zz C zz C C

A xy C xy C C

I I m x y

I I m x y

= + +

= +

mI dv

V

= z pp( ) ( ) = pp p p pp

T TI3

Example 22 2

2

( )

a

zzC

a

I x y dxdydz

= +

5;

1

6Cz zI a=

2

6Cxx Cyy Czz

maI I I= = =

A

c

A

c

A

cx y za

= = =2

m a= 3

222

2 3Axx Ayy Azz Czz

maI I I I ma= = = + =

2

4A x y A x z A y z m aI I I= = =

{A}

a

{C}


Veloc

ities,

accel

and Inertialforces

forces

erations

Newton-Euler Equations

Translational Motion

Cm F=v

Rotational Motion

C CI I N + =

m FC. v =m

CI

Angular Acceleration

{i}

+1

i i i+ += +1 1

i i iZ+ + +=1 1 1

i i i i i i iZ Z+ + + + += + +1 1 1 1 1b g

8/7/2019 good4send

5/10

Linear Acceleration

ZZ+1

x+1

x

p+1a

1 1 1i i i i iv v p V + + += + +V d Z

i i i+ + +=1 1 1

P a x d Z i i i i i+ + += +1 1 1

( )i i i i i i i+ + += + + 1 1 1 v v p p

+ ++ + + +2 1 1 1 1 d Z d Z i i i i i

v v p pi i i i i i iV+ + + += + + +1 1 1 1

Velocity and Accelerationat center of mass

+1 vCi+1Ci+1

pCi+1

vi+

1{i+1}

v v pC i i C i i+ += + + +1 11 1

( )v v p pC i i C i i C i i i+ + += + + + + + +1 1 11 1 1 1

Dynamic forces on Link i

nifi

+fi 1 +ni 1

m vi Ci

Ci i i Ci iI I +

Link i

m forcesi Civ =

/i i i i i iC CI I moments c + = Inertial forces/moments

F mi i Ci

= v

i i i i i iC CN I I = +

ni

fi

+fi 1 +ni 1

pi+1

pCi

Fi

Ni

F f f

N n n f f

i i i

i i i C i i C ii i

=

= + + +

+ + +

1

1 1 1( ) ( ) ( )p p p


Velo

cities

, acce

l

and Inertialforces

forces

erations

Recursive Equations

i i i+ += +1 1 = + + + i i iZ

1 1

( )

( )

i i i i i i i

i i i i i i i i i i i i

C i i C i i C

Z Z

d Z d Z

i i i

+ + + + +

+ + + + + + +

+ + + +

= + +

= + + + +

= + + + + +

1 1 1 1 1

1 1 1 1 1 1 1

1 1 1 1

2

1 1 1

v p p

v p p

where

f F f

n N n F f

i i i

i i i C i i ii

= +

= + + +

+

+ + +

1

1 1 1p p

i

i i

i i

n Z

f Z=

RST

.

.

revolute

prismatic

F mi i Ci= vwithi i i i i iC CN I I = +

8/7/2019 good4send

6/10

i

i i

i i

i i

i

i

i

i i

i i

i i

i i

i

i

i i i

i

i

i

i i

i i

i

i

i

i

i

i

i

i

i

i

i

i

C

i

i

i

C

i

i

i

i

i

C

R Z

R R Z Z

R

i i i

++

++

++

++

+ + ++ + +

++

++

++ +

+ ++

+ ++

++

+

= +

= + +

= + +

= + + +

1

1

1

1

1

1

1

1

1 1 1

1 1 1

1

1

1

1

1

1 1

1 1

1

1 1

1

1

1

1

1 1

( ( ) )

(

v p p v

v p p+

+

+ +

+

=

= +

++

++ +

+

++ +

++

++ +

++

1

1

1 1

1

1

1

1 1

1

1

1 1

1

1

1

1 1

1

1

)

i

i

i

i i

i

C

i

i

C

i

i

i

i

i

C

i

i

i

F m

N I I

i

i i

v

v

Outward iterations: i : 0 5

Inward iterations: i : 6 1i

i i

i i

i

i

i

i

i

i

i i

i i

i

i

C

i

i

i

i i

i i

i

i

i

i

T i

i

f R f F

n N R n F R f

n Z

i

= +

= + + +

=

++

+

++

+ + ++

+

1

1

1

1

1

1 1 1

1

1p p

0

0 1v = GGravity: set

Video Segment

Space Rover, EPFL, Switzerland,ICRA 2000 video proceedings

Lagrange Equations

( )d L L

dt q q

=

L UK=

( )U

q

d K K

dt q q

+ =

Inertial forces Gravity vector

( )U U q=

Kinetic Energy

Potential Energy

Lgrangian

Since

Lagrange Equations

( ) ;d K K

dt q

UG G

qq

= =

Inertial forces

( ) ( , ) ( )M q q V q q G q+ =

Inertial forces1

( )2

TK q M q q=

( ) ( )d K d

M q M q M qdt q dt

= = +

1[ ( ) ]2

TKq M q q

q q

=

( )d K K

dt q qG

=

11

2

T

T

n

Mq q

q

Mq

Mq q

q

M q

=

+

( )M q q=

( , )M q V q q+ ( )d K K

dt q q

=

8/7/2019 good4send

7/10

KM q

q

=

K MT=

1

2

( q q)q

K K v

q v q

=

1/ 2M v Mq= =

M KT= =

1

2

1 2 /v q v v

vv v v( )

1

2

T = M1 2/

2 21 1; ( )

2 2K mx mx mx

x

= =

Equations of Motion

11

2

T

T

n

Mq q

q

Mq

Mq q

q

M q

=

+

( , )M q V q q+ ( )d K K

dt q q

=

( ) ( , ) ( )M q q V q q G q + =+

(( )) ,M V q qq 1

2( ):

TM q qMqK=

Equations of Motionvc

ii

Pci

Link i

KTotal Kinetic Energy:

1

2Lin iT

kMqK K q=

Kinetic Energy

Work done by external forces tobring the system from rest to itscurrent state.

21

2K mv=

CI

1

2

T

CK I =

Fvm

Equations of Motion

1 ( )2 i i

T T

i i C C i C i iK m v v I = +

1

n

i

i

K K=

=

vci

i

Pci

Link i

Explicit Form

Total Kinetic Energy

Equations of Motion

Generalized Coordinatesq

Kinetic EnergyQuadratic Form ofGeneralized Velocities

1

1(

1

2)

2 i i

nT T

i C C i C i i

T

i

m v vq M q I =

+

q

1

2

Tq qK M=

vci

i

Pci

Link i

Explicit Form

Generalized Velocities

8/7/2019 good4send

8/10

Equations of Motion

i ivCJv q=

1

1( )

2 i i i iT T

v v

nT T

i C i

i

m q q qJ J J J I q =

= +

Explicit Formvc

ii

Pci

Link i

iiCJq

=

1

1(

1

2)

2 i i i

nT T

i C C i C i

T

i

m v vq M q I =

= +

Equations of Motionvc

ii

Pci

Link i

Explicit Form

1

2

Tq qM

1

( )1

2 i i i i

nT T

i v

i

T

v C imq J J J J qI =

=

+

1

( )i i i i

nT T

i v v C i

i

M m J J J I J =

+=

Equations of Motion

1 2

[ 0 0 0]ii

i iCC C

i

v

p p

qJ

q

p

q

=

vci

i

Pci

Link i

Explicit Form

i ivCJv q=

iiCJq

=

[ ]1 1 2 2 0 0 0i i iz zJ z

=

( )

11 12 1

21 22 2

1 2

( )n n

n

n

n n nn

m m m

m m mM q

m m m

=

VectorV( , )q q Centrifugal & Coriolis Forces

1 1 1 111 12

12 22 2 2 2 2

q v gm m

m m q v g

+ + =

VectorV( , )q q

V MM

M

m m

m m

m m

m m

m m

m m

T

q

T

q

T

T

= L

NM

O

QP =

FHG

IKJ

FHG

IKJ

FHG

IKJ

L

N

MMMM

O

Q

PPPP

qq q

q qq

q q

q q

1

2

1

2

1

2

11 12

12 22

111 121

121 221

112 122

122 222

11 111 1 112 2m m q m q= +

M

q1

11

1

m

q

22

2

m

q

2111 111 111 122 122 2211

2

2211 211 112 222 222 222

1 1( ) ( )

2 2( , )

1 1( ) ( )

2 2

m m m m m mq

Vq

m m m m m m

+ +

= + +

q q

++

+

LNM

OQP

m m m

m m mq q

112 121 121

212 221 122

1 2

8/7/2019 good4send

9/10

Christoffel Symbolsb m m mijk ijk ikj jki= +

1

2( )

m

q

ij

k

Vb b

b b

q

q

b

bq q=

L

N

MO

Q

PL

N

MO

Q

P +L

N

MO

Q

P111 122

211 222

1

2

2

2

112

212

1 2

2

2

C( )q B ( )q

B

b b b

b b b

b b b

q q

q q

q qn

n n n n

n n

n n

n n n n n n n

( ) [ ]

(

( )) (

( ))

, , ,(

, , ,(

, , ,( (

q qq

=

L

N

MMMM

O

Q

PPPP

L

N

MMMM

O

Q

PPPP

1

2

1

21

2 2 2

2 2 2

2 2 2

1 12 1 13 1 1)

2 12 2 13 2 1)

12 13 1)

1 2

1 3

1)

C

b b b

b b b

b b b

q

q

q

n n n

nn

nn

n n n nn n

( )[ ]

( ) ( )

, , ,

, , ,

, , ,

q q

=

L

N

MMMM

O

Q

PPPP

L

N

MMMM

O

Q

PPPP

2

1 11 1 22 1

2 11 2 22 2

11 22

1

2

2

2

2

1

Potential Energy

hip

Ci

Link iCi

mi

g

U m g h U i i i= +0 0

U m gi iT

Ci= ( );p U Ui

i

= G

U

qm g

qj

j

i

T C

ji

n

i= = =

( )

p

1

Gravity Vector

G J J J

m g

m g

m g

v

T

v

T

v

T

n

n=

F

H

GGGG

I

K

JJJJ

1 2

1

2

d i

Gravity Vector

G J m g J m g J m gvT

v

T

v

T

nn= + + +( ( ) ( ) ( ))

1 21 2

m2g

c2

m1g

c1mng

cnm3g

c3g

1

l1y0

1CI

2CI

m1

m2

d2

x0

1 1 1 1 1 2 2 2 2 21 2

T T T T

v v C v v C M m J J J I J m J J J I J = + + +

Matrix M

Jv1 Jv2and : direct differentiation of the vectors:

0

1 1

1 1

0

2 1

2 11 2

0 0

p pC C

l c

l s

d c

d s=

L

N

MMM

O

Q

PPP

=

L

N

MMM

O

Q

PPP

; and

In frame {0}, these matrices are:

1 2

1 1 2 1 1

0 0

1 1 2 1 1

0

0 ; and

0 0 0 0

v v

l s d s c

J l c J d c s

= =

This yields

m J Jm l

m J Jm d

mv

T

v v

T

v1

0 0 1 1

2

2

0 0 2 2

2

21 1 2 2

0

0 0

0

0( ) ( )=

LNM

OQP =

LNM

OQP; and

g

1

l1

y01C

I

2CI

m1

m2

d2

x0

The matrices and are given byJ1 J2J J

1 21 1 1 1 2 20= = = z z zand

Joint 1 is revolute and joint 2 is prismatic:

1 2

1 1

0 0

0 01 0

J J

= = And

1 1 1 2 2 2

1 21 1 1 1 1 10 0

( ) ; and ( )0 0 0 0

zz zzT T

C C

I IJ I J J I J

= =

Finally,

Mm l I m d I

m

zz zz=+ + +L

NM

OQP

1 1

2

1 2 2

2

2

2

0

0

g

1

l1

y0 1CI

2CI

m1

m2

d2

x0

8/7/2019 good4send

10/10

Centrifugal and Coriolis Vector V

b m m mi jk ijk ikj jki, = + 1

2c h

where mm

qijk

ij

k

=

; withb b i jiii iji= = >0 0and for

For this manipulator, only m11is configuration dependent- function of d2. This implies that only m112is non-zero,

m m d112 2 22= .

Matrix B

Matrix C

Matrix V

Bb m d

=LNM

OQP

=LNM

OQP

2

0

2

0

112 2 2.

Cb

b m d=

LNM

OQP

=

LNM

OQP

0

0

0 0

0

122

211 2 2

.

Vm d

dm d d

=LNM

OQP

+

LNM

OQPL

NM

O

QP

2

0

0 0

0

2 2

1 2

2 2

1

2

2

2

.

g

1

l1

y01C

I

2CI

m1

m2

d2

x0

Vector V

Vm d

dm d d

=LNM

OQP

+

LNM

OQPL

NM

O

QP

2

0

0 0

0

2 2

1 2

2 2

1

2

2

2

.

The Gravity Vector G

G g g= +J m J mvT

v

T

1 21 2 .

In frame {0}, and the gravity vector is0 0 0g = g

Ta f

0 1 1 1 1

1

2 1 2 1

1 1

2

0

0 0 0

0

0

0

0

0

0

G = L

NMOQP

L

N

MMM

O

Q

PPP

L

NMOQP

L

N

MMM

O

Q

PPP

l s l cm g

d s d c

c sm g

0 1 1 2 2 1

2 1

G =+L

NM

OQP

m l m d gc

m gs

b gand

Equations of Motion

m dd

m d d

2 2

1 2

2 2

1

2

2

2

2

0

0 0

0+

LNM

OQP

+

LNM

OQPL

NM

O

QP

++L

NM

OQP =

LNM

OQP

m l m d gc

m gs

1 1 2 2 1

2 1

1

2

b g

.

m l I m d I

m d

zz zz1 1

2

1 2 2

2

2

2

1

2

0

0

+ + +LNM

OQPL

NM

O

QP

g

1

l1

y01C

I

2CI

m1

m2

d2

x0

good4send

Documents