Lecture Notes for AMathematicalIntroduction to ...murray/books/MLS/pdf/zxl_manipdyn-jul09.pdf ·...

Chapter 4RobotDynamics andControl

LagrangianEquations

InertialProperties ofRigid Body

Dynamics ofan Open-chainManipulator

Newton-EulerEquations

Coordinate-invariantalgorithms forrobotdynamics

Lagrange’sEquations withConstraints

Summer School-Math. Methods in [email protected] 13-31 July 2009

Chapter 4 Robot Dynamics and Control

1

Lecture Notes

for

AMathematical Introduction to

Robotic Manipulation

By

Z.X. Li∗ and Y.Q. Wu#

∗Dept. of ECE, Hong Kong University of Science & Technology#School of ME, Shanghai Jiaotong University

24 July 2009


LagrangianEquations






Summer School-Math. Methods in [email protected] 13-31 July 2009


2


1 Lagrangian Equations

2 Inertial Properties of Rigid Body

3 Dynamics of an Open-chain Manipulator

4 Newton-Euler Equations

5 Coordinate-invariant algorithms for robot dynamics

6 Lagrange’s Equations with Constraints


LagrangianEquations






4.1 Lagrangian EquationsChapter 4 Robot Dynamics and Control

3

◻ A Simple Example:

x

y

m

mg

FFx

Fy

♢ Review:Newton’s Equation: Lagrangian Equation:

mx = Fx

my = Fy −mg

Momentum: Px = mx

Py = myddtPx = Fx ,

ddtPy = Fy −mg

⇔

d

dt

∂L

∂x−∂L

∂x= Fx

d

dt

∂L

∂y−∂L

∂y= Fy

Lagrangian function:

L = T −V , Px =∂L∂x , Py =

∂L∂y

Kinetic energy:

T = 12m(x2 + y2)2

Potential energy:

V = mgy


LagrangianEquations







4

◻ Generalization to multibody systems:

x

y

m1

q1

m2

q2

m3 q3

qi, i = 1, . . . , n: generalized coordinatesKinetic energy:

T = T(q, q)Potential energy:

V = V(q)Lagrangian:

L(q, q) = T(q, q) −V(q)τi, i = 1, . . . , n: external force on qiLagrangian Equation:

d

dt

∂L

∂qi−

∂L

∂qi= τi , i = 1, . . . , n


LagrangianEquations







5

◇ Example: Pendulum equationGeneralized coordinate:

θ ∈ S1

Kinematics:

x = l sin θ , y = −l cos θ⇒ x = l cos θ ⋅ θ , y = l sin θ ⋅ θ

Kinetic energy:

T(θ , θ) =1

2m(x2 + y2) = 1

2ml2θ2

Potential energy:

V = mgl(1 − cos θ)

x

y

mg

θ

Lagrangian function:

L = T −V = 1

2ml2θ −mgl(1 − cos θ),⇒ ∂L

∂θ= ml2θ ,

∂L

∂θ= −mgl sin θ

Equation of motion:d

dt

∂L

∂θ− ∂L

∂θ= τ⇒ ml2θ +mgl sin θ = τ


LagrangianEquations







6

◇ Example: Dynamics of a Spherical Pendulum

r(θ , ϕ) =⎡⎢⎢⎢⎣l sin θ cos ϕl sin θ sin ϕ−l cos θ

⎤⎥⎥⎥⎦T =

1

2m∥r∥2 = 1

2ml2(θ2 + (1 − cos2 θ)ϕ2)

V = −mgl cos θ

L(q, q) = 1

2ml2(θ + (1 − cosθ)ϕ2) +mgl cos θ

θ

ϕ mg


LagrangianEquations







7

⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

d

dt

∂L

∂θ=

d

dt(ml2θ) = ml2θ ,

∂L

∂θ= ml2 sin θ cos θϕ2 −mgl sin θ

d

dt

∂L

∂ϕ=

d

dt(ml2 sin2 θϕ) = ml2 sin2 θϕ + 2ml2 sin θ cos θθϕ,

∂L

∂ϕ= 0

[ ml2 00 ml2s2θ

] [ θϕ] + [ −ml2sθcθ ϕ

2

2ml2sθcθ θ ϕ] + [ mglsθ

0 ] = [ 00 ]


LagrangianEquations






4.2 Intertial Properties of Rigid BodyChapter 4 Robot Dynamics and Control

8

◻ Kinetic energy of a rigid body:

A

Br

gab

ra

Volume occupied by the body: VMass density: ρ(r)

Mass: m = ∫Vρ(r)dV

Mass center: r ≜ 1

m ∫Vρ(r)rdV

Relative to frame at the mass center: r = 0


LagrangianEquations







9

In A-frame

T =1

2 ∫V ρ(r)∥p + Rr∥2dV = 1

2 ∫V ρ(r)(∥p∥2 + 2pT Rr + ∥Rr∥2)dV=1

2m∥p∥2 + pTR ∫

Vρ(r)rdV

´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶=0+ 12 ∫ ρ(r)∥Rr∥2dV

1

2 ∫V ρ(r)∥Rr∥2dV=1

2 ∫ ρ(r)∥RT Rr∥2dV = 1

2 ∫ ρ(r)∥ωr∥2dV = 1

2 ∫ ρ(r)∥rω∥2dV=1

2 ∫ ρ(r)(−ωT r2ω)dV = 1

2ωT ( ∫ (−ρ(r)r)2dV)ω ≜ 1

2ωT

Iω


LagrangianEquations







10

where

I = − ∫ ρ(r)r2dV ≜⎡⎢⎢⎢⎢⎣Ixx Ixy IxzIxy Iyy IyzIxz Iyz Izz

⎤⎥⎥⎥⎥⎦with

Ixx = ∫ ρ(r)(y2 + z2)dxdydz, Ixy = − ∫ ρ(r)xydxdydz

T =1

2m∥p∥2 + 1

2(ωb)TIbωb =

1

2m∥RT p∥2 + 1

2(ωb)TIbωb

=1

2(Vb)T [ mI 0

0 Ib ]

´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶Mb

Vb

For Vb = g−1 ⋅ g, with ωb = RT ⋅ R and vb = RT p,Mb is the

Generalized inertia matrix in B frame.


LagrangianEquations







11

◇ Example: Mb for a rectangular object

ρ =m

lωhIxx = ∫

Vρ(y2 + z2)dxdydz

= ρ ∫h2

− h2

∫ω2

− ω2

∫l2

− l2

(y2 + z2)dxdydz= ρ ( 1

12(lω3h + lωh3)) = m

12(ω2 + h2)

Ixy = − ∫Vρxydv = −ρ ∫

h2

− h2

∫ω2

− ω2

∫l2

− l2

xydxdydz

= −ρ ∫h2

− h2

∫ω2

− ω2

y

2x2∣

l2

− l2

dydz = 0

x

y

z

h

w

l


LagrangianEquations







12

I =

⎡⎢⎢⎢⎢⎢⎣

m12(w2 + h2) 0 0

0 m12(l2 + h2) 0

0 0 m12(w2 + l2)

⎤⎥⎥⎥⎥⎥⎦,M = [ mI3×3 0

0 I]

V1 = g−11 ⋅ g1 , M1

T =1

2VT1 M1V1

V1 = Adg0V2

A B

g0

g1(t)

g2(t)

g0

T =1

2(Adg0V2)TM1(Adg0V2) = 1

2VT2 Ad

Tg0M1Adg0V2 ≜ 1

2VT2 M2V2

◻M under change of frames:

M2 = AdTg0M1Adg0


LagrangianEquations







13

◇ Example: Dynamics of a 2-dof planar robot

Ii =⎡⎢⎢⎢⎣Ixxi 0 00 Iyyi 00 0 Izzi

⎤⎥⎥⎥⎦ , i = 1, 2

T(θ , θ) = 1

2m1∥v1∥2 + 1

2ωT1 I1ω1

+ 1

2m2∥v2∥2 + 1

2ωT2 I2ω2

ω1 = [ 00θ1] ω2 = [ 0

0θ1 + θ2 ]

θ1

l1

θ2

l2

r1

r2

x

y


LagrangianEquations







14

Pi = [ xiyi0]: Mass center γi: Distance from joint i to mass center

x1 = r1c1

y1 = r1s1⇒

x1 = −r1s1 θ1y1 = r1c1θ1

x2 = l1c1 + r2c12y2 = l1s1 + r2s12 ⇒

x2 = −(l1s1 + r2s12)θ1 − r2s12θ2y1 = (l1c1 + r2c12)θ1 + r2c12θ2

T(θ , θ) = 1

2m1(x21 + y21 ) + 1

2Iz1 θ

21 + 1

2m2(x22 + y22) + 1

2Iz2(θ21 + θ22)

=1

2[θ1 θ2] [ α + 2βc2 δ + βc2

δ + βc2 δ ]´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶

M(θ)[ θ1θ2]

α = Iz1 + Iz2 +m1r21 +m2(l21 + r22), β = m2l1r2 , δ = Iz2 +m2r

22 , L = T

⇒M(θ) [ θ1θ2] + [ −βs2θ2 −βx

βs2θ1 0] [ θ1

θ2] = [ τ1

τ2 ]


LagrangianEquations






4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control

15

◻Dynamics of open-chain manipulator:

De�nition:

Li: frame at mass center of link i, gsli(θ) = expξ1θ1 ⋯ expξiθ i gsli(o)θ1

θ2

l0

θ3

l1 l2

r2r1

S

r0

L1

L2 L3

Vbsli= Jbsli(θ)θ = [ξ†1 ξ†2 ⋯ ξ†i 0⋯ 0]

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

θ1⋮θiθi+1⋮θn

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦= Ji(θ)θ


LagrangianEquations







16

ξ†j = Ad−1(eξj+1θ j+1⋯eξiθ igsli(0))ξj , j ≤ i

Ti(θ , θ) = 1

2(Vb

sli)TMb

i Vbsli=1

2θTJTi (θ)Mb

i Ji(θ)θT(θ) = n∑

i=1Ti(θ , θ) = 1

2θTM(θ)θ ,

M(θ) =∑i

JTi (θ)Mbi Ji(θ) = 1

2

n∑i,j=1

Mij(θ)θi θjhi(θ): Height of Li, Vi(θ) = mighi(θ), V(θ) =∑

i=1mighi(θ)

Lagrange’s Equation:

d

dt

∂L

∂θi− ∂L

∂θi= Γi, i = 1, . . . , n,

d

dt

∂L

∂θi=

d

dt

⎛⎝

n∑j=1

Mijθj⎞⎠ =

n∑j=1

Mijθj + Mijθj


LagrangianEquations







17

∂L

∂θi=1

2

n∑j,k=1

∂Mkj

∂θiθkθj − ∂V

∂θiMij =∑

k

∂Mij

∂θkθk

⇒n∑j=1

Mijθj + n∑j,k=1(∂Mij

∂θkθjθk − 1

2

∂Mkj

∂θiθkθj) + ∂V

∂θi= Γi

⇒n∑j=1

Mijθj + n∑j,k=1

Γkij θkθj + ∂V

∂θi= Γi

Γkij =1

2(∂Mij

∂θk+ ∂Mik

∂θj− ∂Mkj

∂θi)

θi ⋅ θj , i ≠ j : Coriolis force θ2i : Centrifugal force

De�ne: cij(θ , θ) = n∑k=1

Γkij θk =1

2

n∑k=1(∂Mij

∂θk+ ∂Mik

∂θj− ∂Mkj

∂θi)θk

⇒ M(θ)θ +C(θ , θ)θ +N(θ) = τ


LagrangianEquations







18

Property 1:

1 M(θ) =MT(θ), θTM(θ)θ ≥ 0, θTM(θ)θ = 0⇔ θ = 0

2 M − 2C ∈ Rn×n is skew symmetric

Proof :

(M − 2C)ij = Mij − 2cij(θ)=

n∑k=1

∂Mij

∂θkθk − ∂Mij

∂θkθk − ∂Mik

∂θjθk + ∂Mkj

∂θiθk

=n∑k=1

∂Mkj

∂θiθk − ∂Mik

∂θjθk

Switching i and j shows (M − 2C)T = −(M − 2C)


LagrangianEquations







19

◇ Example: Planar 2-DoF Robot (continued)m11(θ) = α + 2β cos θ2,m22 = δ

m12(θ) = m21(θ) = δ + β cos θ2c11(θ , θ) = −β sin θ2 ⋅ θ2, c12(θ , θ) = −β sin θ2(θ1 + θ2)c21(θ , θ) = β sin θ2 ⋅ θ1 , c22(θ , θ) = 0⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

Γ111 =1

2(∂M11

∂θ1+ ∂M11

∂θ1− ∂M11

∂θ1) = 1

2

∂M11

∂θ1= 0

Γ211 =1

2(∂M11

∂θ2+ ∂M12

∂θ1− ∂M21

∂θ1) = 1

2

∂M11

∂θ2= −β sin θ2

Γ112 =1

2(∂M12

∂θ1+ ∂M11

∂θ2− ∂M12

∂θ1) = 1

2

∂M11


Γ212 =1

2(∂M12

∂θ2+ ∂M12

∂θ2− ∂M22

∂θ1) = ∂M12

∂θ2− 1

2

∂M22



LagrangianEquations







20

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

Γ121 =1

2(∂M21

∂θ1+ ∂M21

∂θ1− ∂M11

∂θ2) = ∂M21

∂θ1− 1

2

∂M11

∂θ2= β sin θ2

Γ221 =1

2(∂M21

∂θ2+ ∂M22

∂θ1− ∂M21

∂θ2) = 1

2

∂M22

∂θ1= 0

Γ122 =1

2(∂M22

∂θ1+ ∂M21

∂θ2− ∂M12

∂θ2) = 1

2

∂M22

∂θ1= 0

Γ222 =1

2(∂M22

∂θ2+ ∂M22

∂θ2− ∂M22

∂θ2) = 1

2

∂M22

∂θ2= 0

M − 2C = [ −2β sin θ2 ⋅ θ2 −β sin θ2 ⋅ θ2−β sin θ2 ⋅ θ2 0]

− [ −2β sin θ2 ⋅ θ2 −2β sin θ2(θ1 + θ2)2β sin θ2 ⋅ θ1 0

]= [ 0 β sin θ2(2θ1 + θ2)−β sin θ2(2θ1 + θ2) 0

] ⇐ skew-symmetric


LagrangianEquations







21

◇ Example: Dynamics of a 3-dof robot

ξ1 =⎡⎢⎢⎢⎢⎢⎢⎢⎣

000001

⎤⎥⎥⎥⎥⎥⎥⎥⎦, ξ2 =

⎡⎢⎢⎢⎢⎢⎢⎢⎣

0−l00−101

⎤⎥⎥⎥⎥⎥⎥⎥⎦, ξ3 =

⎡⎢⎢⎢⎢⎢⎢⎢⎣

0−l0l1−101

⎤⎥⎥⎥⎥⎥⎥⎥⎦

θ1

θ2

l0

θ3

l1 l2

r2r1

S

r0

L1

L2 L3

gsl1(0) =⎡⎢⎢⎢⎢⎣

I ( 00r0)

0 1

⎤⎥⎥⎥⎥⎦, gsl2 (0) =

⎡⎢⎢⎢⎢⎣I ( 0

r1l0)

0 1

⎤⎥⎥⎥⎥⎦, gsl3 (0) =

⎡⎢⎢⎢⎢⎣I ( 0

l1 + r2l0

)0 1

⎤⎥⎥⎥⎥⎦


LagrangianEquations







22

Mi =

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣

mi 0 00 mi 00 0 mi

0

0Ixi 0 00 Iyi 00 0 Izi

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦mi:�e mass of the objectIxi :�e moment of inertia about the x axis

Γ211 = (Iy2 − Iz2 −m2r21 )c2s2 + (Iy2 − Iz3)c23s23 −m3(l1c2 + r2c23)(l1s2 + r2s23)Γ311 = (Iy3 − Iz3)c23s23 −m3r2s23(l1c2 + r2c23)Γ112 = (Iy2 − Iz2 −m2r

21 )c2s2 + (Iy3 − Iz3)c23s23 −m3(l1c2 + r2c23)(l1s2 + r2s23)

Γ113 = (Iy3 − Iz3)c23s23 −m3r2s23(l1c2 + r2c23)Γ121 = (Iz2 − Iy2 +m2r

21 )c2s2 + (Iz3 − Iy3)c23s23 +m3(l1c2 + r2c23)(l1s2 + r2s23)

Γ322 = −l1m3r2s3 , Γ223 = −l1m3r2s3 , Γ323 = −l1m3r2s3Γ131 = (Iz3 − Iy3)c23s23 +m3r2s23(l1c2 + r2c23) , Γ232 = l1m3r2s3


LagrangianEquations







23

N(θ , θ) = ∂V

∂θ, V(θ) = m1gh1(θ) +m2gh2(θ) +m2gh3(θ)

gsli(θ) = eξ1θ1⋯eξiθ igsli(0)⇒h1(θ) = r0 , h2(θ) = l0 − r1 sin θ , h3(θ)

= l0 − l1 sin θ2 − r2 sin(θ2 + θ3)gsli(θ)= eξ1θ1⋯eξiθ igsli(0)⇒

h1(θ) = r0 , h2(θ) = l0 − r1 sin θ , h3(θ)= l0 − l1 sin θ2 − r2 sin(θ2 + θ3)


LagrangianEquations







24

J1 = Jbsl1(θ) =

⎡⎢⎢⎢⎢⎢⎢⎢⎣

0 0 00 0 00 0 00 0 00 0 01 0 0

⎤⎥⎥⎥⎥⎥⎥⎥⎦J2 = J

bsl2(θ) =

⎡⎢⎢⎢⎢⎢⎢⎢⎣

−r1c2 0 00 0 00 −r1 00 −1 0−s2 0 0c2 0 0

⎤⎥⎥⎥⎥⎥⎥⎥⎦

J3 = Jbsl3(θ) =

⎡⎢⎢⎢⎢⎢⎢⎢⎣

−l2c2 − r2c23 0 00 l1s3 00 −r2 − l1c3 −r20 −1 −1−s23 0 0c23 0 0

⎤⎥⎥⎥⎥⎥⎥⎥⎦


LagrangianEquations







25

M(θ) = [ M11 M12 M13M21 M22 M23M31 M32 M33

] = JT1 M1J1 + JT2 M2J2 + JT3 M3J3

M11 = Iy2s22 + Iy3s223 + Iz1 + Iz2c22 + Iz3c223 +m2r

21 c

22 +m3(l1c2 + r2c23)2

M12 =M13 =M21 =M31 = 0

M22 = Ix2 + Ix3 +m2l21 +M2r

21 +m3r

22 + 2m3l1r2c3

M23 = Ix3 +m3r22 +m3l1r2c3

M32 = Ix3 +m3r22 +m3l1r2c3

M33 = Ix3 +m3r22

Cij(θ , θ) = n∑k=1

Γkij θk =1

2

n∑k=1(∂Mij

∂θk+ ∂Mik

∂θj− ∂Mkj

∂θi) θk


LagrangianEquations







26

◻ Additional Properties of the dynamics interms of POE:De�ne:

Aij =

⎧⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎩

Ad−1eξj+1 θj+1⋯e ξi θi i > j

I i = j

0 i < jJi(θ) = Adg−1sli

(0)[Ai1ξ1⋯Aiiξi 0⋯0]M′i = AdTg−1

sli(0)MiAdg−1

sli(0)( intertia of ith link in S)


LagrangianEquations







27

Property 2:

Mij(θ) = n∑l=max(i,j)

ξTi ATliM′lAljξj

Cij(θ , θ) = 1

2

n∑k=1(∂Mij

∂θk+ ∂Mik

∂θj− ∂Mkj

∂θi) θk

where∂Mij

∂θk=

n∑l=max(i,j)

([Ak−1,i ξi, ξk]TATlkM

′lAljξj

+ξTi ATliM′lAlk[Ak−1,j ξj, ξk])


LagrangianEquations






4.4 Newton-Euler EquationsChapter 4 Robot Dynamics and Control

28

◻ Newton-Euler equations in body frame:Newtons Equation:

f =d

dt(mp) = mp

Spatial angular momentum:

I′ ⋅ ωs = R(I ⋅ ωb) = R ⋅ I ⋅ RT´¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¶I′

⋅ωs

S

Tr

g ∶ (R, p)f


LagrangianEquations







29

τ =d

dt(I sωs) = d

dt(RIbRTωs) = I sωs + RIbRTωs + RIbRTωs

= I sωs + RRT±ωs

Isωs − RIbRT ωsωs = I sωs + ωs × (I sωs)

◻ Transformation of all equations totwist/wrench in body frame:

d

dt(mp) = d

dt(mRvb) = mRvb +mRvb,RT f s = mRTRvb +mvb

⇒ f b = mωb × vb +mvb,

τb = RTτs = RT d

dt(RIbωb) = Ibωb + ωb × Ibωb

⇒ [ mI 00 I

b ]´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶

M

[ vb

ωb ]´¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¶

Vb

+ [ ωb ×mvb

ωb × Ibωb ] = [ f b

τb] = Fb(∗)


LagrangianEquations







30

De�ne: se(3) × se(3) ↦ se(3), (ξ1 , ξ2)↦ ξ1 ξ2 − ξ2 ξ1 ≜ ξ, ifξi = [ ωi vi

0 0 ] , i = 1, 2then

ξ = [ (ω1 × ω2)∧ ω1v2 − ω2v10 0 ] = adξi ⋅ ξ2

where

adξ1 = [ ω1 v10 ω1

]�us (∗)⇔MbVb − adVbMbVb = F

b


LagrangianEquations







31

◻ Coordinate invariance of Newton-Eulerequations:

M1V1 − adTV1M1V1 = F1

V1 = Adg0V2

AdTg0F1 = F2⇒

F1 = (AdTg0)−1F2 = (Ad−1g0 )TF2 A

Br

g1

g2

g0

M1 = Ad−Tg0 M2Ad

−1g0

⇒ Ad−Tg0 M2Ad−1g0Adg0 V2 − adT(Adg0 V2)Ad−Tg0 M2Ad

−1g0Adg0V1 = Ad

−Tg0 F2

Since adAdg0V = Adg0adVAd−1g0 , we have

M2V2 − adV2M2V2 = F2


LagrangianEquations






4.5 Coordinate-invariant algorithms for robot dynamics


32

C0

C1

C2

Cn Cn+1

Ci: Frame �xed to link i, located along the ith axisFi: Generalized force link i − 1 exerting on link i, expressed in Ciτi: Joint torque of link i

gi−1,i: Transformation of Ci relative to Ci−1gi−1,i(θi) = eξ′i θ i ⋅ gi−1,i(0) = gi−1,i(0)eξiθ i

ξi = Adg−1i−1,i(0) ⋅ ξ′i : ith axis in Ci frame.

ξ =

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩

⎡⎢⎢⎢⎢⎣0

zi

⎤⎥⎥⎥⎥⎦∶ Revolute joint.

⎡⎢⎢⎢⎢⎣zi

0

⎤⎥⎥⎥⎥⎦∶ Prismatic joint.


LagrangianEquations








33

⇒ g−1i−1,i ⋅ gi−1,i = ξi ⋅ θiMi: Moment of inertia in Ci

Mi = [ miI −mirimiri Ii −mir

2i] mi: Mass of link i

Ii: Angular moment of inertia

gi = gi−1gi−1,iVi = g

−1i ⋅ gi = g−1i−1,iVi−1gi−1,i + ξiθi

Vi = Adg−1i−1,iVi−1 + ξiθi

˙Vi = g−1i−1,iVi−1gi−1,i + g−1i−1,iVi−1gi−1,i + g−1i−1,i ˙Vi−1gi−1,i + ξiθi

= −g−1i−1,i gi−1,ig−1i−1,iVi−1gi−1,i + g−1i−1,iVi−1gi−1,ig−1i−1,i gi−1,i+ g−1i−1,i ˙Vi−1gi−1,i + ξiθi


LagrangianEquations








34

= −ξiθi(Adg−1i−1,iVi−1)∧ + (Adg−1i−1,i

Vi−1)∧ ξiθi + (Adg−1i−1,iVi−1)∧ + ξiθi

⇒ Vi = ξiθi +Adg−1i−1,iVi−1 − adξi θ i(Adg−1i−1,i

Vi−1)◻ Forward Recursion:i = 0 ∶ V0 = 0, V0 = [ g

0 ]⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩

gi−1,i = gi−1,i(0)eξiθ iVi = Adg−1i−1,i

Vi−1 + ξiθiVi = ξiθi +Adg−1i−1,i

Vi−1 − adξi θ i(Adg−1i−1,iVi−1)

◻ Backward Recursion:Fn+1: End-e�ector wrench, gn,n+1: transform from tool frame to Cn

Fi = AdTg−1i,i+1⋅ Fi+1 +MiVi − adTVi

⋅MiVi

τi = ξTi ⋅ Fi


LagrangianEquations








35

V1 = Adg−10,1⋅V0 + ξ1θ1

Fn = AdTg−1n ,n+1⋅ Fn+1 +MnVn − adTVn

⋅ (MnVn)De�ne:

V = [ V1⋮Vn] ∈ R

6n×1, θ =⎡⎢⎢⎢⎢⎣

θ1⋮θn

⎤⎥⎥⎥⎥⎦∈ R

n, ξ = [ ξ1 0 00 ⋱ 00 0 ξn

] ∈ R6n×n

F = [ F1⋮Fn] ∈ R

6n×1, τ = [ τ1⋮τn] ∈ R

n, P0 =

⎡⎢⎢⎢⎢⎢⎣Adg−10,1

0⋮0

⎤⎥⎥⎥⎥⎥⎦∈ R

6n×6

Pt = [0⋯0 AdTg−1n ,n+1] ∈ R

6×6n


LagrangianEquations








36

V1 = Adg−10,1⋅V0 + ξ1θ1

V2 −Adg−11,2V1 = ξ2θ2

⋮

Vn −Adg−1n−1,nVn−1 = ξnθn

⇒

⎡⎢⎢⎢⎢⎢⎢⎣

I 0 ⋯ 0−Adg−11,2

I ⋱ ⋮

0 ⋱ ⋱ 00 0 −Adg−1n−1,n

I

⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶G−1

⎡⎢⎢⎢⎢⎣V1V2⋮Vn

⎤⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¶V

=

⎡⎢⎢⎢⎢⎢⎣Adg−10,1

0⋮0

⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶P0

V0 +

⎡⎢⎢⎢⎢⎢⎣ξ1

ξ2⋱

ξn

⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ξ

⎡⎢⎢⎢⎢⎢⎢⎣

θ1θ2⋮

θn

⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¶θ

�us V = GP0V0 +Gξθ


LagrangianEquations








37

where

G =

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣

I 0 0 ⋯ 0Adg−11,2

I 0 ⋯ 0

Adg−11,3Adg−12,3

I ⋱ ⋮

⋮ ⋮ ⋱ I 0Adg−11,n

Adg−12,n⋯ Adg−1n−1,n

I

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦∈ R

6n×6n

V1 = ξ1θ1 +Adg−10,1V0 − adξ1 θ1(Adg−10,1

V0)V2 −Adg−11,2

V1 = ξ2θ2 − adξ2 θ2(Adg−11,2V1)

Vn −Adg−1n−1,nVn−1 = ξnθn − adξn θn(Adg−1n−1,n

Vn−1)


LagrangianEquations








38

⎡⎢⎢⎢⎢⎢⎢⎣

I 0 ⋯ 0−Adg−11,2I ⋱ ⋮

0 ⋱ ⋱ 00 0 −Adg−1n−1,n

I

⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶G−1

⎡⎢⎢⎢⎢⎢⎣V1V2⋮Vn

⎤⎥⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎢⎣

Adg−10,1

0⋮0

⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶P0

V0+⎡⎢⎢⎢⎢⎢⎣

ξ1ξ2 ⋱

ξn

⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ξ

⎡⎢⎢⎢⎢⎢⎢⎣θ1θ2⋮θn

⎤⎥⎥⎥⎥⎥⎥⎦

+⎡⎢⎢⎢⎢⎢⎢⎣

−adξ1 θ 1 0 ⋯ 00 −adξ2 θ2 ⋱ ⋮⋮ ⋱ ⋱ 00 ⋯ 0 −adξn θn

⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ad

ξ θ

⎡⎢⎢⎢⎢⎢⎣Adg−10,10⋮0

⎤⎥⎥⎥⎥⎥⎦V0

+⎡⎢⎢⎢⎢⎢⎢⎣

−adξ1 θ 1 0 ⋯ 00 −adξ2 θ2 ⋱ ⋮⋮ ⋱ ⋱ 00 ⋯ 0 −adξn θn

⎤⎥⎥⎥⎥⎥⎥⎦

⎡⎢⎢⎢⎢⎢⎢⎣

0 0 ⋯ 0Adg−11,2

0 ⋱ ⋮0 ⋱ ⋱ 00 0 Adg−1n−1,n

0

⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶Γ

⎡⎢⎢⎢⎢⎣V1V2⋮Vn

⎤⎥⎥⎥⎥⎦


LagrangianEquations








39

�us

V = G ⋅ ξθ +G ⋅ P0V0 +G ⋅ adξθP0V0 +G ⋅ adξθΓV

Finally the backward recursion:

Fn = AdTg−1n ,n+1

Fn+1 +MnVn − adTVn⋅ (MnVn)

Fn−1 = AdTg−1n−1,nFn +Mn−1Vn−1 − adTVn−1

⋅ (Mn−1Vn−1)⋮

F1 = AdTg−11,2

F2 +M1V1 − adTV1(M1V1)


LagrangianEquations








40

⇒

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

I −AdTg−11,20 0

0 I ⋱ 0⋮ ⋱ ⋱ −AdTg−1n−1,n0 ⋯ 0 I

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

⎡⎢⎢⎢⎢⎢⎢⎣

F1F2⋮Fn

⎤⎥⎥⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎢⎢⎣

M1M2

⋱Mn

⎤⎥⎥⎥⎥⎥⎥⎦´ ¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸ ¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶M

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣

V1V2⋮

Vn

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦

+

⎡⎢⎢⎢⎢⎢⎣

0⋮0

AdTg−1n ,n+1

⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶PTt

Fn+1 +⎡⎢⎢⎢⎢⎣−adTV1

⋱

−adTVn

⎤⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶adTV

[ M1⋱

Mn][ V1⋮Vn]

F = GTMV +GTPTt Fn+1±Ft

+GT⋅ adTVMV


LagrangianEquations








41

⇒

τ = ξT ⋅ F

τ = ξTGTMV + ξTGTPTt Ft + ξTGT⋅ adTVMV

= ξTGTM(Gξθ +GP0V0 +G ⋅ adξθP0V0 +G ⋅ adξθΓV)+ ξTGTPTt Ft + ξ

TGT⋅ adTVMV

= ξTGTMGξθ + ξTGTMGP0V0 + ξTGTMG ⋅ adξθΓV

+ ξTGTPTt Ft + ξTGT⋅ adTVMV

Finally we get:M(θ)³¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹·¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹µ

ξTGTMGξ θ +

C(θ ,θ)³¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹·¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹µξTGT(MG ⋅ adξθΓ + ad

TVM)Gξ θ

+ ξTGTMGP0V0´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ϕ(θ)

+ ξTGTPTt´¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¶JTt (θ)

Ft = τ


LagrangianEquations








42

M(θ)θ + C(θ , θ) + ϕ(θ) + JTt (θ)Ft = τM(θ) = ξTGTMGξ

C(θ , θ) = ξTGT(MGadξθΓ + adTVM)Gξθ

ϕ(θ) = ξTGTMGP0V0

Jt = PtGξProperty 3:

Γn = 0,

G = (I − Γ)−1 = I + Γ + Γ2 +⋯+ Γn−1I + ΓG = G


LagrangianEquations






4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control

43

De�nition: Holonomic constraints

Given generalized coordinates q = (q1 , . . . , qn) ∈ E, a holonomic

constraint is a set of constraint equations:

hi(q) = 0, i = 1, . . . , kQ = h−1(0) is a manifold of dim n − k ≜ m if the constraints are

linearly independent.

TqQ ∶ {V ∈ TqE∣dhi ⋅V = 0,∀i = 1, . . . , k} ⊂ TqEsubspace of permissible velocities.

T∗qQ⊥ ∶ {f ∈ T∗q E∣⟨f , v⟩ = 0,∀V ∈ TqQ} = span{dh1 , . . . , dhk}subspace of constraint forces.

De�nition: Constraint forces

Γ = ∂h∂q

T⋅ λ

λ ∈ Rk is the vector of relative magnitudes of constraint forces.


LagrangianEquations







44

De�nition: Pfa�an Constraints

A Pfa�an constraint has the form:

A(q)q = 0,A(q) ∈ Rk×n

Given a Pfa�an constraint,

∆q = {Vq ∈ TqE∣A(q) ⋅Vq = 0} ⊂ TqEDistribution of permissible velocities.

De�nition:

A(q)q = 0 is holonomic (or integrable) i� ∆q is an involutive

distribution, or i�

∃hi ∶ E↦ R, i = 1, . . . , k s.t. ∆q = TqQ,Q = h−1(0)


LagrangianEquations







45

§ Constraint forces:

Γ = AT(q) ⋅ λ, λ ∈ Rk

§ Kinetic energy:

T(q, q) = 1

2qT ⋅M(q) ⋅ q

§ Potential energy:

V(q)§ Lagrangian:

L(q, q) = T(q, q) −V(q)


LagrangianEquations







46

◻ Lagrange’s equations with constraints:

M(q)q + C(q, q) +N(q) +AT(q)λ = F◻ Explicit solution for constraint forces:

A(q)q + A(q)q = 0(AM−1AT)λ = AM−1(F − C −N) + Aqλ = (AM−1AT)−1(AM−1(F − C −N) + Aq)


LagrangianEquations







47

◇ Example:

x

y

(x, y)

mg

l

θ

x2 + y2 = l2

[x y]²A(q)

[ xy ] = 0

L(q, q) = 1

2m(x2 + y2) −mgy

[ m 00 m ] [ x

y ] + [ 0mg ] + [ xy ] λ = 0

λ = (AM−1AT)−1(AM−1(F − C −N) + Aq)= −

m

l2(gy + x2 + y2)

= ∥[ xy ] λ∥ = mg

ly +

m

l(x2 + y2)


LagrangianEquations







48

◻ Lagrange-d’Alembert formulation:Given the Pfa�an constraint A(q)q = 0 and virtual displacement

δq ∈ Rk, we have:

�eorem 1 (D’alembert Principle):

Forces of constraints do no virtual work!(AT(q)λ) ⋅ δq = 0 for A(q)δq = 0

1717–1783

�eorem 2 (Lagrange-d’Alembert Equation):

( ddt

∂L

∂q−∂L

∂q− τ) ⋅ δq = 0,A(q)δq = 0

1736-1813


LagrangianEquations







49

Let A(q) = [A1(q) A2(q)], and A2(q) ∈ Rk×k is invertible, then

δq1 ∈ Rn−k are free variables:

δq2 = −A−12 (q)A1(q)δq1

⇒ ( ddt

∂L

∂q−∂L

∂q− τ) ⋅ δq

= ( ddt

∂L

∂q1−

∂L

∂q1− τ1) ⋅ δq1 + ( d

dt

∂L

∂q2−

∂L

∂q2− τ2) ⋅ δq2

= ( ddt

∂L

∂q1−

∂L

∂q1− τ1) ⋅ δq1 + ( d

dt

∂L

∂q2−

∂L

∂q2− τ2) ⋅ (−A−12 A1)δq1


LagrangianEquations







50

As δq1 ∈ Rn−k is free,

( ddt

∂L

∂q1−

∂L

∂q1− τ1) − AT

1 A−T2 ( ddt

∂L

∂q2−

∂L

∂q2− τ2) = 0

Lagrange-d’Alembert equation


LagrangianEquations







51

◇ Example: Dynamics of a rolling disk

Pfa�an constraint:⎧⎪⎪⎨⎪⎪⎩x − ρ cos θ ϕ = 0

y − ρ sin θϕ = 0 (x , y)θ

ϕ

⇒ A(q)q = [ 1 0 0 −ρ cos θ0 1 0 −ρ sin θ ] q = 0

L(q, q) = 12m(x2 + y2) + 1

2I1θ

2 +12I2ϕ

2

Lagrange-d’Alembert equation:

⎛⎝⎡⎢⎢⎢⎢⎣m

mI1

I2

⎤⎥⎥⎥⎥⎦q −

⎡⎢⎢⎢⎢⎣00τθτϕ

⎤⎥⎥⎥⎥⎦⎞⎠ ⋅ δq = 0

where A(q) ⋅ δq = 0.


LagrangianEquations







52

As

{ δx = ρ cos θ ⋅ δϕδy = ρ sin θ ⋅ δϕ

the equation of motion is:

([ 0 0mρcθ mρsθ ] [ x

y ] + [ I1 00 I2

] [ θϕ] − [ τθτϕ ]) ⋅ [ δθ

δϕ ] = 0⇒ [ 0 0

mρcθ mρsθ ] [ xy ] + [ I1 0

0 I2] [ θ

ϕ] = [ τθτϕ ]

As ⎧⎪⎪⎨⎪⎪⎩x = ρ cos θ ⋅ ϕ − ρ sin θ ⋅ θ ϕ

y = ρ sin θ ⋅ ϕ + ρ cos θ ⋅ θ ϕ

⇒ [ I1 00 I2 +mρ2 ] [ θ

ϕ] = [ τθτϕ ]

Solve for (θ(t), ϕ(t)), and then solve for (x(t), y(t)) from:⎧⎪⎪⎨⎪⎪⎩x = ρ cos θ ⋅ ϕ

y = ρ sin θ ⋅ ϕ⇐ 1st order di�erential equation


LagrangianEquations







53

◻ Nature of nonholonomic constraints:

Consider q = (r, s) ∈ R2×R with Pfa�an constraint,

s + aT(r)r = 0, a(r) ∈ R2

Lagrangian

L = L(r, r, s)and constrained Lagrangian

Lc(r, r) = L(r, r,−aT(r)r)⇒ Lagrange’s equation:

d

dt

∂Lc∂ri−∂Lc∂ri= 0, i = 1, 2

⇒d

dt( ∂L∂ri− ai(r)∂L

∂s) − ⎛⎝

∂L

∂ri−∂L

∂s∑j

∂aj∂ri

rj⎞⎠ = 0


LagrangianEquations







54

⇒ ( ddt

∂L

∂ri−∂L

∂ri) − ai(r)( d

dt

∂L

∂s−∂L

∂s) = ∂L

∂s

⎛⎝ai(r) −∑j

∂aj∂ri

rj⎞⎠´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶

≠ 0

(∗)

If the constraint is holonomic, i.e.

ai(r) = ∂h

∂rifor some h ∶ E↦ R

then RHS (right hand side) of (∗) equals

∂L

∂s

⎛⎝∑j

∂2h

∂ri∂rjrj −∑

j

∂2h

∂rj∂rirj⎞⎠ = 0


LagrangianEquations







55

◻Metric, duality and orthogonality on TqE:

qx1⋯

xnTqQ

Q = h−1(0)K =

1

2qTM(q)q

=1

2≪ q, q≫M

TqQ⊥ = {V1 ∈ TqE∣≪ V1,V2 ≫M= V

T1 MV2 = 0,∀V2 ∈ TqQ}

T∗qQ⊥ = {f ∈ T∗q E∣⟨f ,V⟩ = 0,∀V ∈ TqQ} ∶ constraint forces

TqE = TqQ⊕ TqQ⊥,T∗q E = T∗qQ⊕ T∗qQ⊥

De�nition:Mb∶ TqE ↦ T∗q E, ⟨M♭V1 ,V2⟩ = VT

1 MV2 =≪ V1 ,V2 ≫M

M♯ ∶ T∗q E ↦ TqE,M♯ =M♭−1


LagrangianEquations







56

Reciprocal

Property 4:

Under the basis ∂∂qi

and dqi, i = 1, . . . , n of TqE and T∗q Erespectively, the matrix representation ofM♭ andM♯ isM and

M−1 respectively.Property 5:

M♯(T∗qQ) = TqQ

M♯(T∗qQ⊥) = TqQ⊥


LagrangianEquations







57

Givenh ∶E ↦ R

k,m = n − k

h∗ ≜ Tqh ∶TqE ↦ Th(q)Rk

h∗ ≜ T∗q h ∶T∗h(q)Rk ↦ T∗q Ewe haveProperty 6:

kerh∗ = TqQ, h∗(TqQ⊥) = Th(q)Rk, h∗(T∗h(q)Rk) = T∗qQ⊥

T∗q E T∗q Rk

TqE Th(q)Rk

h∗

M♯ M♯2

h∗

M♯2 = h∗ ○M♯ ○ h∗


LagrangianEquations







58

Lemma 1: �e map (I − Pω) ∶ T∗q E ↦ T∗qQ⊥ given by

(I − Pω) = h∗ ○M♭2 ○ h∗ ○M♯is a well-de�ned projection map, with the property:(I − Pω)f1 = 0,∀f1 ∈ T∗qQ

(I − Pω)f2 = f2,∀f2 ∈ T∗qQ⊥Proof :

Given f1 ∈ T∗qQ,M

♯(f1) ∈ TqQ = kerh∗, then (I − Pω)(f1) = 0. Forf2 ∈ T

∗qQ⊥, ∃λ ∈ R

n−m s.t. f2 = h∗λ, and

(I − Pω)f2 = h∗M♭2h∗M♯h∗λ= h∗λ = f2

⇒ Pω ∶ T∗q E ↦ T∗qQ

is a well-de�ned projection map. Similarly,

PT ∶ TqE ↦ TqQ, PT = I −M♯h∗Mb

2h∗


LagrangianEquations







59

and (I − PT) ∶ TqE ↦ TqQ⊥ are projection maps.

Lemma 2:PωM =MPT

Pωh∗ = h∗PT = 0

PT = PTω

For nonholonomic constraints:h∗ ← A(q)h∗ ← A∗(q)

TqQ← ∆q

T∗qQ⊥ ← span{ai(q), i = 1, . . . , k}application in hybrid velocity/force control.


LagrangianEquations







60

◻ Lagrange’s equations of motion:

M(q)q + C(q, q) +N +AT(q)λ = τλ = (AM−1AT)−1(AM−1(τ −C −N) + Aq)

M(q)q + AT(AM−1AT)Aq + PωC + PωN = PωτPω = I −A

T(AM−1AT)−1AM−1C = PωC

N = PωN

τ = Pωτ

Mθ + AT(AM−1AT)−1Aθ = PωMθ ≜ Mθ ∶ intertia forces in T∗qQDe�nition: Dynamics in T∗qQ

Mθ + C + N = τ


LagrangianEquations







61

(I − Pω)(Mq +C +N) = (I − Pω)τ +ATτLet

PT = I −M−1AT(AM−1AT)−1A

and

PωM =MPTthen the dynamics in T∗Q⊥:

M(I − PT)(q +M−1C) = (I − Pω)(τ −N) + ATλ

◻ Geometric Interpretation:∇↔M

Mq +C +N = τ +ATλ⇔M∇qq = τ −N + ATλ∇↔ induced metric on TqQ

S ∶TQ⊗ TQÐÐÐÐÐÐ→ N(Q) :2nd fundamental form

↑ ↑tangent vector �eld normal vector �eld


LagrangianEquations







62

∇XY = ∇XY + S(X,Y)M (I − PT)(q +M−1C)´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶

S(q,q)= (I − Pω)(τ −N) + ATλ

MS(q, q): centrifugal force due to curvature of Q in E

M∇qq = τ − N

MS(q, q) = (I − Pω)(τ −N) + ATλ

⎫⎪⎪⎪⎬⎪⎪⎪⎭for hybrid control design


LagrangianEquations







63

◇ Example: Dynamics of a Spherical Pendulum

K =1

2m(x2 + y2 + z2)

=1

2qTMq

q = (x, y, z)T ,M = mI

h ∶qTq − r2 = 0

A = (x, y, z),M♯2 = AM♯AT = r2/mPω =

1

r2

⎡⎢⎢⎢⎢⎣y2 + z2 −xy −xz−yz x2 + z2 −yz−zx −zy x2 + y2

⎤⎥⎥⎥⎥⎦I − Pω =

1

r2

⎡⎢⎢⎢⎢⎣x2 xy xzyx y2 yzzx zy z2

⎤⎥⎥⎥⎥⎦


LagrangianEquations







64

PT = PTω = Pω(µ, ν): Spherical coordinates

q = (r cos µ cos ν, r cos µ sin ν, r sin µ)T∇qq = PT(∇qq)= [ −r sin µ cos ν −r sin ν−r sin µ sin ν r cos ν

r cos µ 0] [ v1

v2 ]S(q, q) = (I − PT)(∇qq)

= (−µ2 − cos2 µν2)[ r cos µ cos νr cos µ sin ν

r sin µ]

wherev1 = µ + sin µ cos µν2

v2 = cos µν − 2 sin µµν


LagrangianEquations







65

◻ Control Algorithm:

1 holonomic constraints:q ∶ coordinates of Q

q = ψ(q)⇒ q = J ⋅ ˙q

τ =MJ(¨qd − Kv˙e − Kpe) + C1 +N +AT(−λd +KI ∫ (λ − λd))

2 nonholonomic constraints:

Let J(q) ∈ Rn×m be s.t. AJ = 0. Write q = J ⋅ u for some u

τ =MJ(ud −Kp(u − ud)) +MJu + C +N + AT(−λd + KI ∫ (λ − λd))


LagrangianEquations







66

◇ Example: 6-DoF manipulator on a sphere withfrictionless point contact

Contact constraint:

vz = 0⇔ [0 0 1 0 0 0]Adg−1flf

Vof = 0

⇒Holonomic constraint:

η = (αTo , α

Tf ,ψ): Parametrization of Q

Pω = diag(1, 1, 0, 1, 1, 1)Newton-Euler Equations of motion:

MVof − adTVof

MVof = Fm +G +ATλ

Vof =

⎡⎢⎢⎢⎢⎢⎣RψMo −Mf 00 0 0

RψRoKoMo −RoKfMf 0−ToMo −TfMf 1

⎤⎥⎥⎥⎥⎥⎦⎡⎢⎢⎢⎣αoαfψ

⎤⎥⎥⎥⎦ ≜ JηMJη +C1 = Fm +G +A

Tλ ∗


LagrangianEquations







67

Pω(∗) ∶Mη + C1 = B1Fm + B1G

−ϕ3 − λ = b2Fm + b2G

F = [f1 f2 f4 f5 f6]T = M(ηd −Kv˙e − Kpe) + C1 − B1G

f3 = −ϕ3 − λd + KI ∫ (λ − λd) − b2Gτ = JTs Fm


LagrangianEquations







68

◇ Example: 6-DoF manipulator rolling on a sphere⎡⎢⎢⎢⎢⎣

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 0 0 1

⎤⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶A1(q)

Vlolf = 0

fc = AT1 λ, λ ∈ R

4

[ ωxωy ] = −R0(Kf + RψKoRψ)Mf αf

Vof = Adgflf ⋅Vlo lf

= Adgflf

⎡⎢⎢⎢⎢⎢⎢⎣

000

−Ro(Kf + RψKoRψ)Mfo

⎤⎥⎥⎥⎥⎥⎥⎦αf

≜ Jf αfspan{Jf }: Not involutive


LagrangianEquations







69

MJf αf + (MJf αf − adTJf αf

MJf αf ) = Fm +G + ATλ

Fm =MJf (αfd − Kp(αf − αfd)) + (MJf αf − adTJf αf

MJf αf )+ AT(−λd + ∫ (λ − λd)) −G

◇ Example: Redundant parallel manipulatorθ = (θ1 , . . . , θ6) ∈ Eθa = (θ1 , θ3, θ5)θp = (θ2, θ4, θ6)

H(θ) =⎡⎢⎢⎢⎢⎢⎣xa + lc1 + lc12 − xb − lc3 − lc34ya + ls1 + ls12 − yb − ls3 − ls34xa + lc1 + lc12 − xc − lc5 − lc56ya + ls1 + ls12 − yc − ls5 − ls56

⎤⎥⎥⎥⎥⎥⎦= 0


LagrangianEquations







70

Mi(θ) ∈ R2×2: ith chain

M(θ) = diag(M1(θ), . . . ,M3(θ))M(θ)θ + C +N = τ + ATλIf all joints are actuated,

Position control of end-e�ector+

internal grasping forceAs τ2, τ4, τ6 = 0,

θ ∈ R2∶ local parametrization of Q = H−1(0)

θ = ψ(θ) ∶embedding of Q in E

θ = J ˙θ


LagrangianEquations







71

Given Pω ∶ T∗θ E ↦ T∗θQ, the dynamics in T∗θQ is given by:

PωMJ ¨θ + Pω(C1 +N) = Pωττ = (τ1 , τ3, τ5)

Pω = (P1 , P3, P5)τ = Pω τ = Pωτ ∈ R

6

τ = PωMJ( ¨θd −Kv˙e −Kpe) + Pω(C1 +N)

Lecture Notes for AMathematicalIntroduction to ...murray/books/MLS/pdf/zxl_manipdyn-jul09.pdf ·...

Documents

Transcript of Lecture Notes for AMathematicalIntroduction to ...murray/books/MLS/pdf/zxl_manipdyn-jul09.pdf ·...