RD4 Dynamics L1 - ETH ZKinematics Marco Hutter Robot Dynamics - Multi-body Dynamics 1215.10.2015 Free Cut Cart pendulum (1) (2) (3) cc x cc y l r c cc r l mx F my F F F mg Fb Fb (4)

||Autonomous Systems Lab

151-0851-00 VLecture: Tuesday 10:15 – 12:00 CAB G11Exercise: Tuesday 14:15 – 16:00 every 2nd week

Marco Hutter, Michael Blösch, Roland Siegwart, Konrad Rudin and Thomas Stastny

15.10.2015Marco Hutter Robot Dynamics - Multi-body Dynamics 1

Robot Dynamics Multi-body Dynamics

||Autonomous Systems Lab 15.10.2015Marco Hutter Robot Dynamics - Multi-body Dynamics 2

New program is online


Kinematics = description of motions Translations and rotations Various representations (Euler, quaternions, etc.) Instantaneous kinematics Jacobians and basic Jacobians Inverse kinematics and control Outlook to floating base systems (unactuated base and contacts)


Recapitulation of Kinematics


Dynamics in Robotics


Description of “cause of motion” Input Force/Torque acting o system Output Motion of the system

Principle of virtual work Newton’s law for particles Conservation of impulse and angular momentum

3 methods to get the EoM Free cut and conservation of impulse & angular momentum for each body Projected Newton-Euler (generalized coordinates) Lagrange II (energy)

Introduction to control methods Inverse dynamics and operational space control

Introduction to dynamics of floating base systems External forces


DynamicsOutline

,b g M

Generalized coordinates Mass matrix

, Centrifugal and Coriolis forces

Gravity forces Generalized forces

b

g

M


Principle of virtual work (D’Alembert’s Principle) Dynamic equilibrium imposes zero virtual work

Newton’s law for every particle in direction it can move


Fundamentals 1Principle of virtual work

dF external forces acting on element

acceleration of element mass of element

virtual displacement

i

idm i

0T

System

W dm dF

dynamicdF

dm

F ma

S

F ma

p

dF mv pdt

Impulse orlinear momentum

force

N

d mv Ndt

angular momentummoment


Rigid body Kinematics (Lecture 1&2)


Fundamentals 2Rigid body Kinematics

dm

I

S

OSr

OSr

S OSv r

S S OSa v r

IB

Sr

0T

S

W dm dF

Sr

Sa

Sv


Rigid body Kinematics (Lecture 1&2)

Applied to principle of virtual work


Fundamentals 2Rigid body Kinematics

dm

I

S

OSr

3 3

3 3

3 3

OS

SS

SS

SS

rv

v

aa

rr

I ρ

I ρ ΩΩ

I ρ

S OSv r

S S OSa v r

IB

Sr

0T

S

W dm dF

,3 3

,

with: ( ( )

0

( ( )) a

0

nd

0T aS S

K

T

K

a

r m a F

a b b a b a a b a a

IΘ ΩΘ

: (body mass)dm m: 0 (since S=CoG)dm

: (Inertia matrix around CoG)TSdm ρρ Θ

3 3 ,3 3

,3 3

,

0T

S S a K

K

T T a KS S

a KK

r aW dm dm dF

r adm dm dm dFdm dm dm dF

II ρ ΩΩ

ρ

I ρ ΩΩρ ρρ ρΩΩ ρ


Use the following definitions

Change in linear impulse

Change in angular momentum (around CoG)

Conservation of impulse and angular momentum


Fundamentals 3Impulse and angular momentum

Sp ma

S SN Θ Θ

,

,0

T a KS S

a K

pr rFWN

External forces and moments

Change in impulse and angular momentum

Newton

Euler

A free body can move In all directions


Cut all bodies free Introduction of constraining force Apply conservation and to individual bodies

System of equations 6n equation Eliminate all constrained forces (5n)

Pros and Cons+ Intuitively clear + Direct access to constraining forces− Becomes a huge combinatorial problem for large MBS


1st Method for EoMFree cut of all bodies of MBS

I

iOPr

ivia

gF

1iF

2iF

im


Find the equation of motion


Free CutCart pendulum

g

,p pm

,c cm

Il


Impulse / angular momentum cart

Impulse / angular momentum pendulum

Kinematics



(1)(2)

(3)

c c x

c c y l r c

c c r l

m x Fm y F F F m g

F b Fb

(4)

(5)

cos sin (6)

p p x

p p y p

p p x p y p

m x F

m y F m g

F l F l

(7)0 (constraint) (8)

0 (constraint) (9)

c

p

p

x xy

sin (10 )

cos (11 )

(12 )

p

p

p

x x l a

y l a

a

2

2

cos sin (10)

sin cos (11)

(12)

p

p

p

x x l l

y l l

g

,p pm

,c cm

pm g

lF rF

xF

yF

yF

xFI

x

l

6 equations, 6 unknowns resp.12 equations, 12 unknowns

How many dimensions does the EoM have?

cm g


(7),(10-12) in (1) and (4-6)

From (13) and (14) remove Fx

Insert (13) and (15) in (16) to remove Fx and Fy



2

2

(13)

cos sin (14)

sin cos (15)

cos sin (16)

c x

p x

p y p

p x y

m x F

m x l l F

m l l F m g

F l F l

2cos sin 0p c p pm m x m l lm

2 cos sin 0p p p pm l m l x glm

g

,p pm

,c cm

pm g

lF rF

xF

yF

yF

xFI

x

l


For multi-body systems

Express the impulse/angular momentum in generalized coordinates

Virtual displacement in generalized coordinates

With this, the principle of virtual work transforms to


Fundamentals 4Generalized coordinates and Jacobians

,

,all bodies consistent with kinematics

0 T a K

S Sa K

pr rFWN

PS

R

v

JJ

PS P

R R

a

J JJ J

P PS

R R R RS

p m mma

N

J JΘJΘ Θ ΘJ J ΘJ

M , b g

0(unactuated)

,

,all bodies

0 W= T T T a K

P P P P PTa K

R R R RR R R

m m F

J J J J JJ ΘJ J JΘJ J ΘJ

PS

R

r

JJ


Equation of motion Directly get the dynamic properties of a multi-body system with n bodies

For actuated systems, include actuation force as external force for each body If actuators act in the direction of generalized coordinates, corresponds to stacked

actuator commands


2nd Method for EoMProjected Newton-Euler

1

1

1

,

i i i i i

i i i i i i i i i

i

nT Ts i S R S R

in

T T Ts i S R S R R R S R

i

nT gs i

i

m

b m

g

M J J J Θ J

J J J Θ J J J Θ J

J F

,b g M




Projected Newton-EulerCart pendulum

g

,p pm

,c cm

Il


Kinematics cart and pendulum

Equation of motion


Projected Newton-EulerCart pendulum

x

0

1 00 0

0 00 0

c

c

OS

OSPc

PcPc

xr

drd

dddt

J

JJ

sincos

1 cos0 sin

0 sin0 cos

p

p

OS

OSPp

PpPp

x lr

l

dr lld

d ld

ldt

J

JJ

2

coscosi i i i

c p pT T T T TP i P R i R Pc c Pc Pp p Pp Rp p Rp

p p p

m m lmm m m

lm m l

M J J J J J J J J J J

0 1

p

pRp

J

i i i i i i

T TP i P R i R R i Rb m J J J Θ J J Θ J

20 (planar system) sin0

T pPp p Pp

lmm

J J

1

0 00sini c p

nT g T Ts i P P

i p pc

gm g m glm g

J F J J

g

,p pm

,c cm

I

x

l


Lagrangian equation

Lagrangian

Since


3rd Method for EoMLagrange II

d L Ldt

d K K Udt

L UK kinetic energy

potential energy

inertial forces gravity vector 12

TK M

11 ,

2

T

T

n

g gb

M

MMMM

with K

M

,

U U

K K


Kinetic energy in joint space

Kinetic energy for all bodies

From kinematics we know that

Hence we get


3rd Method for EoMLagrange II

12

TK M

1 12 2

T Ti i i i i iK r m r Θ

and i ii P i Rr J J

12 i i i i

T T TP i P R i RK m J J J Θ J

mass matrix




Lagrange IICart pendulum

g

,p pm

,c cm

Il


Kinematics cart and pendulum

Kinetic and potential energy

Equation of motion


Lagrange IICart pendulum

0

0

c

c

OS

S

xr

xr

sincos

cossin

p

p

OS

S

x lr

l

x lr

l

p

p

2 2 2 2 21 1 1 1 1 1cos2 2 2 2 2 2i i

T TS i S i i i c p p pK r m r m x m x m l m xl

cospU m gl

0d K K Udt

2

coscos

c p p

p p

m x m x m lKm l m xl

2

2

cos sin0

cos sinc p p p

p p p

m x m x m l m lm l m xl m gl

g

,p pm

,c cm

I

x

l0-level is can be chosen

2

2

cos sincos sin

c p p p

p p p

m x m x m l m ld Km l m xl m xldt

0

sinp

Km xl

0sinp

Um gl


Joint-space dynamics End-effector dynamics

Torque to force mapping

Kinematic relation

15.10.2015Marco Hutter Robot Dynamics - Legged Robots 22

Joint Space and Operational Space Dynamics

I

EEF

Ex

b g M

1 TE E E E Ex F b g J M J J

E Ex p F Λ

E E Ex J J

TE EF J

1

1 1 TE E E E E Ex b g F

Λ

J M J J M J

11

1

1

TE E

E

E

b

p g

Λ J M J

ΛJ M ΛJ

ΛJ M

E Ex J


End-effector dynamics in contact

Make sure that end-effector acceleration in contactfor direction is zero (often with selection matrices)[see Springer Handbook of Robotics]

15.10.2015Marco Hutter Robot Dynamics - Legged Robots 23

Joint Space and Operational Space DynamicsOperational Space Control (OSC)

I

EF

Ex

contac EtE F Fx p Λ

,ˆ T

des E E desF J

contactF

F M M S S I S

, ,,ˆ ˆˆ

E deE s F cond tact des M esFF px SΛS


Given:

Find , s.t. the end-effector accelerates with exerts the contact force

24

Operational Space Control2-link example

, 0c

contact des

FF

2

2 P

O

b g M

, 0 Tcontact des cF F

, 0T

E des yr a

,

0E des

y

ra

15.10.2015Marco Hutter Robot Dynamics - Legged Robots


Given:

Find , s.t. the end-effector accelerates with exerts the contact force

End-effector position and Jacobian

Desired end-effector dynamics

25

Operational Space Control2-link example

, 0c

contact des

FF

,

0E des

y

ra

2

2 P

O

b g M

, 0T

E des yr a

, 0 Tcontact des cF F

1 12

1 12E

ls lsr

lc lc

1 12 12

1 12 12E

lc lc lcls ls ls

J 11

1

1

TE E

E

E

b

p g

Λ J M J

ΛJ M ΛJ

ΛJ M

,, ,ˆ ˆˆ

E des contE d ace ds se erF p F Λ

15.10.2015Marco Hutter Robot Dynamics - Legged Robots


Operational Space Control in Robotics

Documents

RD4 Dynamics L1 - ETH ZKinematics Marco Hutter Robot Dynamics - Multi-body Dynamics 1215.10.2015 Free Cut Cart pendulum (1) (2) (3) cc x cc y l r c cc r l mx F my F F F mg Fb Fb (4)