Chapter 4 Robot Dynamics and Control

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

1 Lecture Notes for A Mathematical Introduction to Robotic Manipulation

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator

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

Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

∗

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

24 July 2009

Chapter 4 Robot Dynamics and Control

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

2

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Chapter 4 Robot Dynamics and Control 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

Chapter 4 Robot Dynamics and Control

4.1 Lagrangian Equations ◻ A Simple Example: Chapter 4 Robot Dynamics and Control Lagrangian Equations

m

Newton’s Equation: m¨x = Fx

Dynamics of an Open-chain Manipulator

Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

m¨y = Fy − mg Momentum: Px = m˙x d dt Px

=

Py = m˙y d Fx , dt Py = Fy − mg

Fy

F Fx

mg

♢ Review:

Inertial Properties of Rigid Body

Newton-Euler Equations

3

y

⇔

Lagrangian Equation: d ∂L ∂L − = Fx dt ∂˙x ∂x d ∂L ∂L − = Fy dt ∂˙y ∂y Lagrangian function: L = T − V, Px = ∂L ∂˙x , Py = Kinetic energy: T = 21 m(˙x2 + y˙2 )2 Potential energy: V = mgy

x

∂L ∂˙y

Chapter 4 Robot Dynamics and Control

4.1 Lagrangian Equations 4 ◻ Generalization to multibody systems: Chapter 4 Robot Dynamics and Control

y m3

Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

q3

m2 q2 m1

qi , i = 1, . . . , n: generalized coordinates Kinetic 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 qi Lagrangian Equation:

q1 x

d ∂L ∂L − = τi , i = 1, . . . , n dt ∂˙qi ∂qi

Chapter 4 Robot Dynamics and Control

4.1 Lagrangian Equations 5 ◇ Example: Pendulum equation Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

y Generalized coordinate: 1 θ∈S Kinematics: x x = l sin θ, y = −l cos θ ˙ y˙ = l sin θ ⋅ θ˙ ⇒ x˙ = l cos θ ⋅ θ, Kinetic energy: θ 1 1 2 ˙2 2 2 ˙ ˙ T(θ, θ) = m(˙x + y ) = ml θ 2 2 Potential energy: mg V = mgl(1 − cos θ) Lagrangian function: 1 ∂L ˙ ∂L = −mgl sin θ L = T − V = ml2 θ˙ − mgl(1 − cos θ), ⇒ = ml2 θ, 2 ∂θ ∂ θ˙ Equation of motion: d ∂L ∂L − = τ ⇒ ml2 θ¨ + mgl sin θ = τ dt ˙ ∂θ

Chapter 4 Robot Dynamics and Control

4.1 Lagrangian Equations 6 ◇ Example: Dynamics of a Spherical Pendulum Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎡ l sin θ cos ϕ ⎤ ⎢ ⎥ r(θ, ϕ) = ⎢ l sin θ sin ϕ ⎥ ⎢ −l cos θ ⎥ ⎣ ⎦ 1 1 T = m∥˙r∥2 = ml2 (θ˙2 + (1 − cos2 θ)ϕ˙2 ) 2 2 V = −mgl cos θ 1 L(q, q˙ ) = ml2 (θ˙ + (1 − cosθ )ϕ˙2 ) + mgl cos θ 2

θ

ϕ

mg

Chapter 4 Robot Dynamics and Control

4.1 Lagrangian Equations 7

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎧ d ∂L d ⎪ ˙ = ml2 θ, ¨ ∂L = ml2 sin θ cos θ ϕ˙2 − mgl sin θ ⎪ = (ml2 θ) ⎪ ⎪ ⎪ ˙ ∂θ ⎪ dt ∂ θ dt ⎨ d d ∂L ⎪ ⎪ ˙ = ml2 sin2 θ ϕ¨ + 2ml2 sin θ cos θ θ˙ϕ, ˙ ∂L = 0 ⎪ = (ml2 sin2 θ ϕ) ⎪ ⎪ ˙ ⎪ ∂ϕ ⎩ dt ∂ ϕ dt [

ml2 0 ][ 0 ml2 s2θ

θ¨ ] + [ −ml2 sθ cθ ϕ˙2 ] + [ mglsθ ] = [ 0 ] 0 0 ϕ¨ 2ml2 sθ cθ θ˙ ϕ˙

Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body 8 ◻ Kinetic energy of a rigid body: Chapter 4 Robot Dynamics and Control

r

Lagrangian Equations Inertial Properties of Rigid Body

B

ra A

gab

Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Volume occupied by the body: Mass density: Mass: Mass center: Relative to frame at the mass center:

V ρ(r) m= ρ(r)dV V 1 r≜ ρ(r)rdV m V r=0

∫

∫

Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body 9 In A-frame Chapter 4 Robot Dynamics and Control

1 ˙ 2 dV = 1 ˙ + ∥Rr∥ ˙ 2 )dV ρ(r)∥p˙ + Rr∥ ρ(r)(∥p˙ ∥2 + 2p˙ T Rr 2 V 2 V 1 1 ˙ ˙ 2 dV = m∥p˙ ∥2 + p˙ T R ρ(r)rdV + ρ(r)∥Rr∥ 2 2 V ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶

Lagrangian Equations

Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

∫

∫

Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator

∫

T=

∫

=0

1 2

∫

V

1 = 2 1 = 2

˙ 2 dV ρ(r)∥Rr∥ 1

1

˙ dV = ˆ dV = ∫ ρ(r)∥ˆr ω∥ dV ρ(r)∥ωr∥ ∫ ρ(r)∥R Rr∥ 2 ∫ 2 1 1 ∫ ρ(r)(−ω ˆr ω)dV = 2 ω ( ∫ (−ρ(r)ˆr) dV) ω ≜ 2 ω I ω T

2

T 2

2

T

2

2

T

Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body 10 where Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

I =− with Ixx =

∫

∫ ρ(r)(y

2

⎡ Ixx Ixy Ixz ⎢ ρ(r)ˆr2 dV ≜ ⎢ ⎢ Ixy Iyy Iyz ⎢ Ixz Iyz Izz ⎣ + z2 )dxdydz, Ixy = −

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

∫ ρ(r)xydxdydz

1 1 1 1 T = m∥p˙ ∥2 + (ωb )T I b ωb = m∥RT p˙ ∥2 + (ωb )T I b ωb 2 2 2 2 1 b T mI 0 = (V ) [ ] Vb 0 Ib 2 ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹¶ Mb

˙ and vb = RT p˙ , M b is the For Vˆ b = g −1 ⋅ g˙ , with ωˆ b = RT ⋅ R Generalized inertia matrix in B frame.

Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body 11 ◇ Example: M b for a rectangular object Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

z

m lωh Ixx = ρ(y2 + z2 )dxdydz

ρ=

∫

y

V

=ρ

h 2

ω 2

l 2

− h2

− ω2

− 2l

∫ ∫ ∫

= ρ(

h

1 m (lω3 h + lωh3 )) = (ω2 + h2 ) 12 12

Ixy = − = −ρ

(y2 + z2 )dxdydz

∫

V

ρxydv = −ρ

h 2

ω 2

− h2

− ω2

∫ ∫

l

h 2

ω 2

l 2

− h2

− ω2

− 2l

∫ ∫ ∫

y 2 2 x ∣ dydz = 0 2 −l 2

xydxdydz

x l w

Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎡ ⎢ ⎢ I =⎢ ⎢ ⎢ ⎣

m 2 12 (w

+h ) 0 0 2

Vˆ 1 = g1−1 ⋅ g˙1 , M1 1 T = V1T M1 V1 2 V1 = Adg0 V2

m 2 12 (l

0 + h2 ) 0

⎤ ⎥ ⎥ ⎥ , M = [ mI03×3 I0 ] m 2 2 ⎥ ⎥ 12 (w + l ) ⎦ 0 0

g0 A

B

g1 (t)

g2 (t)

g0

1 1 1 T = (Adg0 V2 )T M1 (Adg0 V2 ) = V2T AdTg0 M1 Adg0 V2 ≜ V2T M2 V2 2 2 2

◻ M under change of frames: M2 = AdTg0 M1 Adg0

12

Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body 13 ◇ Example: Dynamics of a 2-dof planar robot Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎡ Ixxi ⎢ Ii = ⎢ 0 ⎢ 0 ⎣

0 Iyyi 0

0 0 Izzi

⎤ ⎥ ⎥ , i = 1, 2 ⎥ ⎦

˙ = 1 m1 ∥v1 ∥2 + 1 ωT I1 ω1 T(θ, θ) 2 2 1 1 1 + m2 ∥v2 ∥2 + ωT2 I2 ω2 2 2

l2 r 2 θ2

l1

y r1

x

0 ω1 = [ 0 ] θ˙1

ω2 = [

0 0 ] θ˙1 + θ˙2

θ1

Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body 14 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

xi Pi = [ yi ]: Mass center γi : Distance from joint i to mass center 0 x˙ 1 = −r1 s1 θ˙1 x1 = r1 c1 ⇒ y1 = r1 s1 y˙1 = r1 c1 θ˙1 x˙ 2 = −(l1 s1 + r2 s12 )θ˙1 − r2 s12 θ˙2 x2 = l1 c1 + r2 c12 ⇒ y2 = l1 s1 + r2 s12 y˙1 = (l1 c1 + r2 c12 )θ˙1 + r2 c12 θ˙2 ˙ = 1 m1 (x˙ 2 + y˙2 ) + 1 Iz θ˙2 + 1 m2 (x˙ 2 + y˙2 ) + 1 Iz (θ˙2 + θ˙2 ) T(θ, θ) 1 2 2 1 2 2 2 1 1 2 2 2 1 ˙ 1 α + 2βc δ + βc2 = [θ˙1 θ˙2 ] [ δ + βc 2 ] [ θ˙ 1 ] δ 2 θ2 2 ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ α=

Iz1 + Iz2 + m1 r12

+ m2 (l12

¨ ⇒ M(θ) [ θ¨ 1 θ2

M(θ) + r22 ), β

= m2 l1 r2 , δ = Iz2 + m2 r22 , L = T ˙ −βs2 θ˙2 −βx ]+[ ] [ θ˙ 1 ] = [ ττ21 ] ˙ βs2 θ 1 0 θ2

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 15 ◻ Dynamics of open-chain manipulator: Chapter 4 Robot Dynamics and Control

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

Lagrangian Equations

l2 L3

L2

Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator

θ3 l1

r1 l0

r2

L1 r0 S

Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎡ ⎢ ⎢ ⎢ ⎢ b b † † † Vsli = Jsli (θ)θ˙ = [ξ1 ξ2 ⋯ ξi 0 ⋯ 0] ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

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

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

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 16 ξj† = Ad−1 (eξj+1 θ j+1 ⋯eξi θ i gsli (0))ξj , j ≤ i ˆ

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

ˆ

˙ = 1 (V b )T M b V b = 1 θ˙T J T (θ)M b Ji (θ)θ˙ Ti (θ, θ) i sli i i 2 sli 2 n ˙ = 1 θ˙T M(θ)θ, ˙ T(θ) = ∑ Ti (θ, θ) 2 i=1

1 n ∑ Mij (θ)θ˙i θ˙j 2 i,j=1 Vi (θ) = mi ghi (θ), V(θ) = ∑ mi ghi (θ)

M(θ) = ∑ JiT (θ)Mib Ji (θ) = i

hi (θ): Height of Li ,

Lagrange’s Equation: d ∂L ∂L − = Γi , i = 1, . . . , n, dt ∂ θ˙i ∂θ i

i=1

⎞ n d ∂L d ⎛n ˙ ij θ˙j = Mij θ˙j = ∑ Mij θ¨j + M ∑ dt ∂ θ˙i dt ⎝ j=1 ⎠ j=1

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 17 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

∂L 1 n ∂Mkj ˙ ˙ ∂V = ∑ θkθj − ∂θ i 2 j,k=1 ∂θ i ∂θ i n

˙ ij = ∑ M k

∂Mij ˙ θk ∂θ k

n

∂Mij ˙ ˙ 1 ∂Mkj ˙ ˙ ∂V ⇒ ∑ Mij θ¨j + ∑ ( θj θk − θk θj) + = Γi 2 ∂θ i ∂θ i j=1 j,k=1 ∂θ k n n ∂V ⇒ ∑ Mij θ¨j + ∑ Γijk θ˙k θ˙j + = Γi ∂θ i j=1 j,k=1

Γijk =

1 ∂Mij ∂Mik ∂Mkj ( + − ) 2 ∂θ k ∂θ j ∂θ i

θ˙i ⋅ θ˙j , i ≠ j : Coriolis force

θ˙i2 : Centrifugal force n n ˙ = ∑ Γk θ˙k = 1 ∑ ( ∂Mij + ∂Mik − ∂Mkj )θ˙k Define: cij (θ, θ) ij 2 k=1 ∂θ k ∂θ j ∂θ i k=1 ˙ θ˙ + N(θ) = τ ⇒ M(θ)θ¨ + C(θ, θ)

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 18 Property 1: Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

1 2

M(θ) = M T (θ), θ˙T M(θ)θ˙ ≥ 0, θ˙T M(θ)θ˙ = 0 ⇔ θ = 0 ˙ − 2C ∈ Rn×n is skew symmetric M

Proof : ˙ − 2C)ij = M ˙ ij − 2cij (θ) (M n ∂M ∂Mkj ˙ ∂Mij ˙ ∂Mik ˙ ij ˙ =∑ θk − θk − θk + θk ∂θ k ∂θ j ∂θ i k=1 ∂θ k n

=∑ k=1

∂Mkj ˙ ∂Mik ˙ θk − θk ∂θ i ∂θ j

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

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 19 ◇ Example: Planar 2-DoF Robot (continued)

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

m11 (θ) = α + 2β cos θ 2 , m22 = δ

m12 (θ) = m21 (θ) = δ + β cos θ 2 ˙ = −β sin θ 2 ⋅ θ˙2 , c12 (θ, θ) ˙ = −β sin θ 2 (θ˙1 + θ˙2 ) c11 (θ, θ)

˙ = β sin θ 2 ⋅ θ˙1 , c22 (θ, θ) ˙ =0 c21 (θ, θ)

⎧ ⎪ ⎪ Γ111 = ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ Γ2 = ⎪ ⎪ ⎪ 11 ⎨ ⎪ ⎪ 1 ⎪ Γ12 = ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ 2 ⎪ ⎪ Γ12 = ⎪ ⎪ ⎩

1 ∂M11 ∂M11 ∂M11 1 ∂M11 ( + − )= =0 2 ∂θ 1 ∂θ 1 ∂θ 1 2 ∂θ 1 1 ∂M11 ∂M12 ∂M21 1 ∂M11 ( + − )= = −β sin θ 2 2 ∂θ 2 ∂θ 1 ∂θ 1 2 ∂θ 2 1 ∂M11 1 ∂M12 ∂M11 ∂M12 ( + − )= = −β sin θ 2 2 ∂θ 1 ∂θ 2 ∂θ 1 2 ∂θ 2 1 ∂M12 ∂M12 ∂M22 ∂M12 1 ∂M22 ( + − )= − = −β sin θ 2 2 ∂θ 2 ∂θ 2 ∂θ 1 ∂θ 2 2 ∂θ 1

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 20 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎧ 1 ⎪ ⎪ Γ21 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ 2 ⎪ ⎪ Γ ⎪ ⎪ ⎪ 21 ⎨ ⎪ ⎪ 1 ⎪ Γ22 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ 2 ⎪ ⎪ Γ22 ⎪ ⎪ ⎩

1 ∂M21 ∂M21 ∂M11 ∂M21 1 ∂M11 + − )= − = β sin θ 2 = ( 2 ∂θ 1 ∂θ 1 ∂θ 2 ∂θ 1 2 ∂θ 2 1 ∂M21 ∂M22 ∂M21 1 ∂M22 = ( + − )= =0 2 ∂θ 2 ∂θ 1 ∂θ 2 2 ∂θ 1 1 ∂M22 ∂M21 ∂M12 1 ∂M22 = ( + − )= =0 2 ∂θ 1 ∂θ 2 ∂θ 2 2 ∂θ 1 1 ∂M22 ∂M22 ∂M22 1 ∂M22 = ( + − )= =0 2 ∂θ 2 ∂θ 2 ∂θ 2 2 ∂θ 2 ˙ ˙ ˙ − 2C = [ −2β sin θ 2 ⋅ θ 2 −β sin θ 2 ⋅ θ 2 ] M −β sin θ 2 ⋅ θ˙2 0 −2β sin θ 2 ⋅ θ˙2 −2β sin θ 2 (θ˙1 + θ˙2 ) −[ ] 2β sin θ 2 ⋅ θ˙ 1 0 0 β sin θ 2 (2θ˙1 + θ˙2 ) =[ ] ⇐ skew-symmetric ˙ ˙ −β sin θ 2 (2θ 1 + θ 2 ) 0

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 21 ◇ Example: Dynamics of a 3-dof robot Chapter 4 Robot Dynamics and Control

θ1

⎡ ⎢ ⎢ Lagrangian ⎢ Equations ξ 1 = ⎢ ⎢ ⎢ ⎢ Inertial ⎣ Properties of Rigid Body

Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

0 0 0 0 0 1

⎤ ⎡ 0 ⎥ ⎢ −l0 ⎥ ⎢ ⎥ ⎢ 0 ⎥ , ξ 2 = ⎢ −1 ⎥ ⎢ ⎥ ⎢ 0 ⎥ ⎢ 1 ⎦ ⎣

⎤ ⎡ 0 ⎥ ⎢ −l0 ⎥ ⎢ ⎥ ⎢ l1 ⎥ , ξ 3 = ⎢ −1 ⎥ ⎢ ⎥ ⎢ 0 ⎥ ⎢ ⎦ ⎣ 1

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

θ2

θ3 l1

l2 L3

L2 r1 l0

r2

L1 r0 S

⎡ ⎢ I gsl1 (0) = ⎢ ⎢ ⎢ 0 ⎣

⎤ ⎡ 0 ⎢ ( 0 ) ⎥ ⎥, gsl (0) = ⎢ I ⎥ 2 ⎢ r0 ⎥ ⎢ 0 1 ⎦ ⎣

⎤ ⎡ 0 ⎢ ( r1 ) ⎥ ⎥, gsl (0) = ⎢ I ⎥ 3 ⎢ l0 ⎥ ⎢ 0 1 ⎦ ⎣

0 ⎤ ( l 1 + r2 ) ⎥ ⎥ ⎥ l0 ⎥ 1 ⎦

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 22 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎡ mi 0 0 ⎤ ⎢ 0 mi 0 ⎥ 0 ⎢ ⎥ ⎢ 0 0 mi ⎥ Mi = ⎢ Ixi 0 0 ⎥ ⎢ ⎥ ⎢ 0 Iy i 0 ⎥ 0 ⎢ ⎥ ⎢ 0 0 Iz i ⎥ ⎣ ⎦ mi : The mass of the object Ixi : The moment of inertia about the x axis Γ112 = (Iy2 − Iz2 − m2 r12 )c2 s2 + (Iy2 − Iz3 )c23 s23 − m3 (l1 c2 + r2 c23 )(l1 s2 + r2 s23 ) Γ113 = (Iy3 − Iz3 )c23 s23 − m3 r2 s23 (l1 c2 + r2 c23 ) 1 Γ12 = (Iy2 − Iz2 − m2 r12 )c2 s2 + (Iy3 − Iz3 )c23 s23 − m3 (l1 c2 + r2 c23 )(l1 s2 + r2 s23 ) 1 Γ13 = (Iy3 − Iz3 )c23 s23 − m3 r2 s23 (l1 c2 + r2 c23 ) 1 Γ21 = (Iz2 − Iy2 + m2 r12 )c2 s2 + (Iz3 − Iy3 )c23 s23 + m3 (l1 c2 + r2 c23 )(l1 s2 + r2 s23 ) 3 2 3 Γ22 = −l1 m3 r2 s3 , Γ23 = −l1 m3 r2 s3 , Γ23 = −l1 m3 r2 s3 2 Γ311 = (Iz3 − Iy3 )c23 s23 + m3 r2 s23 (l1 c2 + r2 c23 ) , Γ32 = l 1 m3 r2 s 3

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 23 ˙ = ∂V , V(θ) = m1 gh1 (θ) + m2 gh2 (θ) + m2 gh3 (θ) N(θ, θ) ∂θ Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

gsli (θ) = eξ1 θ 1 ⋯eξi θ i gsli (0) ⇒ ˆ

ˆ

h1 (θ) = r0 , h2 (θ) = l0 − r1 sin θ, h3 (θ)

= l0 − l1 sin θ 2 − r2 sin(θ 2 + θ 3 )gsli (θ)

= eξ1 θ 1 ⋯eξi θ i gsli (0) ⇒ ˆ

ˆ

h1 (θ) = r0 , h2 (θ) = l0 − r1 sin θ, h3 (θ)

= l0 − l1 sin θ 2 − r2 sin(θ 2 + θ 3 )

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 24 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎡ ⎢ ⎢ ⎢ b J1 = Jsl1 (θ) = ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ⎢ ⎢ ⎢ J2 = Jslb2 (θ) = ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ⎢ ⎢ ⎢ b J3 = Jsl3 (θ) = ⎢ ⎢ ⎢ ⎢ ⎣

0 0 0 ⎤ 0 0 0 ⎥ ⎥ 0 0 0 ⎥ 0 0 0 ⎥ ⎥ 0 0 0 ⎥ 1 0 0 ⎥ ⎦ −r1 c2 0 0 ⎤ 0 0 0 ⎥ ⎥ 0 −r1 0 ⎥ 0 −1 0 ⎥ ⎥ −s2 0 0 ⎥ c2 0 0 ⎥ ⎦ −l2 c2 − r2 c23 0 0 ⎤ 0 l1 s3 0 ⎥ ⎥ 0 −r2 − l1 c3 −r2 ⎥ 0 −1 −1 ⎥ ⎥ −s23 0 0 ⎥ ⎥ c23 0 0 ⎦

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 25 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

M11 M12 M13 M(θ) = [ M21 M22 M23 ] = J1T M1 J1 + J2T M2 J2 + J3T M3 J3 M31 M32 M33 2 M11 = Iy2 s2 + Iy3 s223 + Iz1 + Iz2 c22 + Iz3 c223 + m2 r12 c22 + m3 (l1 c2 + r2 c23 )2

M12 = M13 = M21 = M31 = 0

M22 = Ix2 + Ix3 + m2 l12 + M2 r12 + m3 r22 + 2m3 l1 r2 c3 M23 = Ix3 + m3 r22 + m3 l1 r2 c3 M32 = Ix3 + m3 r22 + m3 l1 r2 c3 M33 = Ix3 + m3 r22

n n ˙ = ∑ Γk θ˙k = 1 ∑ ( ∂Mij + ∂Mik − ∂Mkj ) θ˙k Cij (θ, θ) ij 2 k=1 ∂θ k ∂θ j ∂θ i k=1

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 26 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

◻ Additional Properties of the dynamics in terms of POE: Define:

⎧ ⎪ Ad−1ξˆj+1 θ j+1 ξˆ θ i > j ⎪ ⎪ ⋯e i i ⎪ ⎪ e Aij = ⎨I i=j ⎪ ⎪ ⎪ ⎪ ⎪ i
sl i

=

AdTg−1 Mi Adg −1 sl (0) sl (0) i

i

( intertia of ith link in S)

Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator 27 Property 2: Mij (θ) =

Chapter 4 Robot Dynamics and Control

Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

ξiT ATli Ml′ Alj ξj

l=max(i,j)

n ˙ = 1 ∑ ( ∂Mij + ∂Mik − ∂Mkj ) θ˙k Cij (θ, θ) 2 k=1 ∂θ k ∂θ j ∂θ i

Lagrangian Equations Inertial Properties of Rigid Body

n

∑

where

n ∂Mij = ∑ ([Ak−1,i ξi , ξk ]T ATlk Ml′ Alj ξj ∂θ k l=max(i,j)

+ξiT ATli Ml′ Alk [Ak−1,j ξj , ξk ])

Chapter 4 Robot Dynamics and Control

4.4 Newton-Euler Equations 28 ◻ Newton-Euler equations in body frame: Newtons Equation: Chapter 4 Robot Dynamics and Control Lagrangian Equations

f = Spatial angular momentum:

Inertial Properties of Rigid Body

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

Dynamics of an Open-chain Manipulator

I′

Newton-Euler Equations

r

Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

d (mp˙ ) = mp¨ dt

S

T f

g ∶ (R, p)

Chapter 4 Robot Dynamics and Control

4.4 Newton-Euler Equations 29 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

d s s d ˙ b RT ωs + RI b R ˙ T ωs (I ω ) = (RI b RT ωs ) = I s ω˙ s + RI dt dt ˙ T I s ωs − RI b RT ωˆ s ωs = I s ω˙ s + ωs × (I s ωs ) = I s ω˙ s + RR ± s

τ=

ωˆ

◻ Transformation of all equations to twist/wrench in body frame:

d d ˙ b + mR˙vb , RT f s = mRT Rv ˙ b + m˙vb (mp˙ ) = (mRvb ) = mRv dt dt ⇒ f b = mωb × v˙b + m˙vb , d τb = RT τs = RT (RI b ωb ) = I b ω˙ b + ωb × I b ωb dt b b b b mI 0 ˙ v ⇒[ ] + [ ωb × mv ] = [ f b ] = F b (∗) b ][ b b b 0 I ω˙ ω ×I ω τ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹¶ ´¹¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹¶ M

Vb

Chapter 4 Robot Dynamics and Control

4.4 Newton-Euler Equations 30 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

ˆ if Define: se(3) × se(3) ↦ se(3), ( ξˆ1 , ξˆ2 ) ↦ ξˆ1 ξˆ2 − ξˆ2 ξˆ1 ≜ ξ, ξˆi = [ ωˆ0i v0i ] , i = 1, 2 then ∧ ωˆ 2 v1 ] = ad ⋅ ξ ξˆ = [ (ω1 ×0ω2 ) ωˆ 1 v2 − 2 ξi 0 where vˆ1 ] ad ξ1 = [ ωˆ01 ω ˆ1 Thus ˙ b − adV b Mb Vb = F b (∗) ⇔ Mb V

Chapter 4 Robot Dynamics and Control

4.4 Newton-Euler Equations 31 ◻ Coordinate invariance of Newton-Euler equations: Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

r

˙ 1 − adTV M1 V1 = F1 M1 V 1 V1 = Adg0 V2 AdTg0 F1 = F2 ⇒ T A F1 = (AdTg0 )−1 F2 = (Ad−1 g 0 ) F2

−1 M1 = Ad−T g0 M2 Adg0 −T −1 ⇒ Adg0 M2 Adg0 Adg0 V˙ 2 − adT(Adg

g0

g2

B

g1

0

−T −1 V˙ 2 ) Adg0 M2 Adg0 Adg0 V1

Since adAdg0 V = Adg0 adV Ad−1 g0 , we have M2 V˙ 2 − adV2 M2 V2 = F2

= Ad−T g 0 F2

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

32 C2

C1

Cn

Cn+1

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

C0

Frame fixed to link i, located along the ith axis Generalized force link i − 1 exerting on link i, expressed in Ci Joint torque of link i Transformation of Ci relative to Ci−1 ˆ′ ˆ gi−1,i (θ i ) = eξi θ i ⋅ gi−1,i (0) = gi−1,i (0)eξi θ i ξi = Adg −1 (0) ⋅ ξi′ : ith axis in Ci frame. i−1,i ⎡ ⎤ ⎧ ⎪ ⎢ 0 ⎥ ⎪ ⎪ ⎢ ⎥∶ ⎪ Revolute joint. ⎪ ⎢ ⎥ ⎪ ⎪ zi ⎥ ⎪⎢ ⎦ ξ = ⎨⎣ ⎡ ⎤ ⎪ ⎢ ⎥ ⎪ z i ⎪ ⎢ ⎥∶ ⎪ Prismatic joint. ⎪ ⎢ ⎥ ⎪ ⎪ ⎢ ⎥ 0 ⎪ ⎩⎣ ⎦

Ci : Fi : τi : gi−1,i :

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

33

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

−1 ⇒ gi−1,i ⋅ g˙i−1,i = ξˆi ⋅ θ˙i

Mi : Moment of inertia in Ci mI −mi ˆri mi : Mi = [ m iˆr I − m ri2 ] Ii : i i i iˆ gi = gi−1 gi−1,i

−1 ˆ Vˆ i = gi−1 ⋅ g˙i = gi−1,i Vi−1 gi−1,i + ξˆi θ˙i Vi = Ad −1 Vi−1 + ξi θ˙i gi−1,i

Mass of link i Angular moment of inertia

−1 ˆ −1 ˆ −1 ˙ V˙ˆ i = g˙i−1,i Vi−1 gi−1,i + gi−1,i Vi−1 g˙i−1,i + gi−1,i Vˆ i−1 gi−1,i + ξˆi θ¨i −1 −1 ˆ −1 ˆ −1 = −gi−1,i g˙i−1,i gi−1,i Vi−1 gi−1,i + gi−1,i Vi−1 gi−1,i gi−1,i g˙i−1,i −1 ˙ + gi−1,i Vˆ i−1 gi−1,i + ξˆi θ¨i

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

34

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

= − ξˆi θ˙i (Adg −1 Vi−1 )∧ + (Adg −1 Vi−1 )∧ ξˆi θ˙i + (Adg −1 V˙ i−1 )∧ + ξˆi θ¨i i−1,i i−1,i i−1,i ⇒ V˙ i = ξi θ¨i + Ad −1 V˙ i−1 − ad ˙ (Ad −1 Vi−1 ) gi−1,i

ξi θ i

gi−1,i

◻ Forward Recursion:

g i = 0 ∶ V0 = 0, V˙ 0 = [ 0 ]

ˆ ⎧ ⎪ gi−1,i = gi−1,i (0)eξi θ i ⎪ ⎪ ⎪ ⎪ ⎪ ˙ −1 Vi−1 + ξ i θ i ⎨ Vi = Adgi−1,i ⎪ ⎪ ⎪ ⎪ ¨ ˙ ˙ i−1 − ad ˙ (Adg −1 Vi−1 ) ⎪ −1 V ⎪ ξi θ i ⎩ Vi = ξi θ i + Adgi−1,i i−1,i

◻ Backward Recursion: Fn+1 : End-effector wrench, gn,n+1 : transform from tool frame to Cn Fi = AdTg−1 ⋅ Fi+1 + Mi V˙ i − adTVi ⋅ Mi Vi i,i+1

τi =

ξiT

⋅ Fi

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

35

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

˙ V1 = Adg0,1 −1 ⋅ V0 + ξ 1 θ 1

Fn = AdTg−1 ⋅ Fn+1 + Mn V˙ n − adTVn ⋅ (Mn Vn ) n,n+1 Define: ⎡ θ˙ ⎤ V1 ξ1 0 ⎢ 1 ⎥ n ⎥ V = [ ⋮ ] ∈ R6n×1 , θ˙ = ⎢ ⎢ ⋮ ⎥∈R ,ξ=[ 0 ⋱ ⎢ θ˙n ⎥ Vn 0 0 ⎣ ⎦ ⎡ Adg −1 ⎢ 0,1 F1 τ1 ⎢ F = [ ⋮ ] ∈ R6n×1 , τ = [ ⋮ ] ∈ Rn , P0 = ⎢ 0 ⎢ ⋮ τn Fn ⎢ 0 ⎣ T 6×6n Pt = [0⋯0 Adg −1 ] ∈ R n,n+1

0 0 ] ∈ R6n×n ξn

⎤ ⎥ ⎥ ⎥ ∈ R6n×6 ⎥ ⎥ ⎦

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

36 ˙ V1 = Adg0,1 −1 ⋅ V0 + ξ 1 θ 1 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

V2 − Adg −1 V1 = ξ2 θ˙2 1,2

⋮ Vn − Adg −1 Vn−1 = ξn θ˙n n−1,n ⎡ I 0 ⋯ 0 ⎤ ⎢ ⎥ ⎡ V1 ⎤ I ⋱ ⋮ ⎥⎢ V −1 ⎢ −Adg1,2 ⎥ ⎢ ⎥⎢ 2 ⎥ ⇒⎢ ⎥⎢ ⋮ ⎥ 0 ⋱ ⋱ 0 ⎢ ⎢ ⎥ 0 0 −Adg −1 I ⎥ ⎢ ⎥ ⎣ Vn ⎦ n−1,n ⎣ ⎦ ´¹¹ ¹ ¹ ¹¸ ¹ ¹ ¹ ¹ ¶ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ V G−1

⎡ Adg −1 ⎤ ⎡ ξ1 ⎤⎡ θ˙1 ⎤ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎢ ˙2 ⎥ ⎢ 0 0,1 ⎥ ⎢ ⎥ ξ θ ⎢ ⎥ 2 =⎢ ⎥V + ⎢ ⎥⎢ ⋱ ⎢ ⋮ ⎥ 0 ⎢ ⎥⎢ ⋮ ⎥ ⎥ ⎢ 0 ⎥ ⎢ ξn ⎥ ⎣ ⎦ ⎣ ⎦⎢ θ˙n ⎥ ⎣ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ´¹¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹⎦ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ¶ P0

Thus V = GP0 V0 + Gξ θ˙

ξ

θ˙

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

37 where Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎡ I ⎢ Ad −1 ⎢ g1,2 ⎢ Ad G=⎢ g −1 ⎢ ⎢ ⋮ 1,3 ⎢ ⎢ Adg −1 ⎣ 1,n

0 I Adg −1 2,3 ⋮ Adg −1

0 ⋯ 0 ⋯ I ⋱ ⋱ I ⋯ Adg −1

n−1,n

2,n

0 0 ⋮ 0 I

⎤ ⎥ ⎥ ⎥ ⎥ ∈ R6n×6n ⎥ ⎥ ⎥ ⎥ ⎦

V˙ 1 = ξ1 θ¨1 + Adg −1 V˙ 0 − ad ξ1 θ˙1 (Adg −1 V0 ) 0,1

0,1

˙ 1 = ξ2 θ¨2 − ad ˙ (Adg −1 V1 ) −1 V V˙ 2 − Adg1,2 ξ2 θ 2 1,2 V˙ n − Adg −1 V˙ n−1 = ξn θ¨n − ad ξn θ˙n (Adg −1 Vn−1 ) n−1,n

n−1,n

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

38

Chapter 4 Robot Dynamics and Control Lagrangian Equations

I 0 ⋯ 0 ⎤⎡ ⎡ ⎢ −Ad −1 I ⋱ ⋮ ⎥ ⎢ ⎥⎢ g 1,2 ⎢ ⎥⎢ ⎢ ⎥⎢ 0 ⋱ ⋱ 0 ⎢ ⎥⎢ 0 0 −Adg −1 I ⎥⎢ ⎢ ⎣ ⎦⎣ n−1,n ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ G−1

Dynamics of an Open-chain Manipulator

Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⎤ ⎡ Adg −1 ⎤ ⎡ ξ1 ⎤⎡ ⎢ ⎥⎢ ⎥ ⎢ 0,1 ⎥ ⎥ ⎢ ⎥˙ ⎢ ⎥⎢ ξ2 0 ⎥⎢ ⎥=⎢ ⎥ V0 +⎢ ⋱ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⋮ ⎢ ⎥ ⎢ ⎥ ⎢ ξn ⎥ 0 ⎦ ⎣ ⎦ ⎣ ⎦⎢ ⎣ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶

θ¨1 θ¨2 ⋮ θ¨n

ξ

P0

⎤ ⎡ Ad −1 ⎡ −ad ξ θ˙ 0 ⋯ 0 ⎥⎢ ⎢ 1 1 g 0,1 ⎢ ⎥⎢ 0 −ad ξ 2 θ˙ 2 ⋱ ⋮ ⎥⎢ ⎢ 0 +⎢ ⎥⎢ ⋮ ⋱ ⋱ 0 ⋮ ⎢ ⎥⎢ ⎢ 0 ⋯ 0 −ad ξ n θ˙n ⎥ 0 ⎣ ⎦⎣ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶

Inertial Properties of Rigid Body

Newton-Euler Equations

V˙ 1 V˙ 2 ⋮ ˙n V

⎤ ⎥ ⎥ ⎥ V0 ⎥ ⎥ ⎦

ad ξ θ˙

⎡ −ad ξ θ˙ ⎢ 1 1 ⎢ 0 +⎢ ⎢ ⋮ ⎢ ⎢ 0 ⎣

0 −ad ξ 2 θ˙ 2 ⋱ ⋯

⋯ ⋱ ⋱ 0

0 ⋮ 0 −ad ξ n θ˙n

0 0 ⋯ 0 ⎤ ⎤⎡ ⎥ ⎢ Ad −1 0 ⎡ V1 ⋱ ⋮ ⎥ ⎥⎢ ⎥⎢ V g 1,2 ⎥⎢ ⎥⎢ 2 ⎥⎢ ⎥ 0 ⋱ ⋱ 0 ⎢ ⎥⎢ ⎢ ⋮ 0 0 Adg −1 0 ⎥ ⎥⎢ ⎥ ⎣ Vn ⎦⎣ ⎦ n−1,n ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ Γ

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

39

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Thus ˙ 0 + G ⋅ ad ˙ P0 V0 + G ⋅ ad ˙ ΓV V˙ = G ⋅ ξ θ¨ + G ⋅ P0 V ξθ ξθ Finally the backward recursion: Fn = AdTg−1 Fn+1 + Mn V˙ n − adTVn ⋅ (Mn Vn ) n,n+1

Fn−1 =

AdTg−1 Fn n−1,n

+ Mn−1 V˙ n−1 − adTVn−1 ⋅ (Mn−1 Vn−1 )

⋮ F1 = AdTg−1 F2 + M1 V˙ 1 − adTV1 (M1 V1 ) 1,2

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

40

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator

⎡ ⎢ ⎢ ⎢ ⎢ ⇒⎢⎢ ⎢ ⎢ ⎢ ⎢ ⎣

⎤ ⎥ ⎡ I −AdTg−1 0 0 ⎥ ⎡ F1 ⎤ ⎡ M 1 ⎤⎢ 1,2 ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ F2 ⎥ ⎢ ⎥⎢ 0 I ⋱ 0 M2 ⎥⎢ ⎥=⎢ ⎥⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⋱ ⎥⎢ ⋮ ⋱ ⋱ −AdTg−1 ⎥⎥⎢⎢ ⋮ ⎥⎥ ⎢⎢ ⎥⎢ M ⎦⎢ n−1,n ⎥⎣ Fn ⎦ ⎣ n ⎥ ⎣ 0 ⋯ 0 I ⎦ ´¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ M 0 ⎤ ⎡ ⎤ ⎡ −adT ⎥ ⎢ ⋮ ⎥ M1 ⎢ V1 ⎢ ⎥ ⎥[ ⋱ ⋱ ⎥ Fn+1 + ⎢ +⎢ 0 ][ ⎢ ⎥ ⎢ T ⎥ T Mn ⎥ ⎢ ⎢ Adg −1 ⎥ −ad Vn ⎦ ⎣ ⎣ n,n+1 ⎦ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ T PtT

adV

Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

F = GT M V˙ + GT PtT Fn+1 +GT ⋅ adTV MV ± Ft

V˙ 1 V˙ 2 ⋮ ˙n V

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

V1 ⋮ ] Vn

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

41 τ = ξT ⋅ F Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

⇒

˙ + ξ T GT PtT Ft + ξ T GT ⋅ adTV MV τ = ξ T GT M V = ξ T GT M(Gξ θ¨ + GP0 V˙ 0 + G ⋅ ad ξ θ˙ P0 V0 + G ⋅ ad ξ θ˙ ΓV) + ξ T GT PtT Ft + ξ T GT ⋅ adTV MV = ξ T GT MGξ θ¨ + ξ T GT MGP0 V˙ 0 + ξ T GT MG ⋅ ad ξ θ˙ ΓV

+ ξ T GT PtT Ft + ξ T GT ⋅ adTV MV Finally we get: ˙ C(θ,θ)

³¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ · ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹µ ³¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ·¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ µ ξ T GT MGξ θ¨ + ξ T GT (MG ⋅ ad ξ θ˙ Γ + adTV M)Gξ θ˙ + ξ T GT MGP0 V˙ 0 + ξ T GT PtT Ft = τ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹¶ ´¹¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¶ M(θ)

ϕ(θ)

JtT (θ)

Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

42 ˙ + ϕ(θ) + J T (θ)Ft = τ M(θ)θ¨ + C(θ, θ) t

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

M(θ) = ξ T GT MGξ ˙ = ξ T GT (MGad ˙ Γ + adT M)Gξ θ˙ C(θ, θ) V ξθ ˙0 ϕ(θ) = ξ T GT MGP0 V

Jt = Pt Gξ Property 3: Γn = 0,

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

I + ΓG = G

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 43 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Definition: Holonomic constraints Given generalized coordinates q = (q1 , . . . , qn ) ∈ E, a holonomic constraint is a set of constraint equations: hi (q) = 0, i = 1, . . . , k −1 Q = h (0) is a manifold of dim n − k ≜ m if the constraints are linearly independent. Tq Q ∶ {V ∈ Tq E∣dhi ⋅ V = 0, ∀i = 1, . . . , k} ⊂ Tq E subspace of permissible velocities. Tq∗ Q⊥ ∶ {f ∈ Tq∗ E∣⟨f , v⟩ = 0, ∀V ∈ Tq Q} = span{dh1 , . . . , dhk } subspace of constraint forces. Definition: Constraint forces Γ= k

∂h T ∂q

⋅λ

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

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 44 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Definition: Pfaffian Constraints A Pfaffian constraint has the form: A(q)˙q = 0, A(q) ∈ Rk×n Given a Pfaffian constraint, ∆q = {Vq ∈ Tq E∣A(q) ⋅ Vq = 0} ⊂ Tq E Distribution of permissible velocities. Definition: A(q)˙q = 0 is holonomic (or integrable) iff ∆q is an involutive distribution, or iff ∃hi ∶ E ↦ R, i = 1, . . . , k s.t. ∆q = Tq Q, Q = h−1 (0)

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 45 § Constraint forces:

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

Chapter 4 Robot Dynamics and Control Lagrangian Equations

§ Kinetic energy: 1 T(q, q˙ ) = q˙ T ⋅ M(q) ⋅ q˙ 2

Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator

§ Potential energy: V(q)

Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

§ Lagrangian:

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

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 46 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

◻ Lagrange’s equations with constraints: M(q)¨q + C(q, q˙ ) + N(q) + AT (q)λ = F

◻ Explicit solution for constraint forces: ˙ q=0 A(q)¨q + A(q)˙

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

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

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 47 ◇ Example: Chapter 4 Robot Dynamics and Control Lagrangian Equations

x2 + y2 = l 2 [x y] [ xy˙˙ ] = 0 ²

y

A(q)

Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

x

l θ

(x, y) mg

1 L(q, q˙ ) = m(˙x2 + y˙2 ) − mgy 2 x¨ x 0 0 [ m 0 m ] [ y¨ ] + [ mg ] + [ y ] λ = 0 ˙ q) λ = (AM −1 AT )−1 (AM −1 (F − C − N) + A˙ m = − 2 (gy + x˙ 2 + y˙2 ) l mg m x = ∥[ y ] λ∥ = y + (˙x2 + y˙2 ) l l

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 48 ◻ Lagrange-d’Alembert formulation: Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body

Given the Pfaffian constraint A(q)˙q = 0 and virtual displacement δq ∈ Rk , we have: Theorem 1 (D’alembert Principle): Forces of constraints do no virtual work! (AT (q)λ) ⋅ δq = 0 for A(q)δq = 0

Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

1717–1783

Theorem 2 (Lagrange-d’Alembert Equation): d ∂L ∂L ( − − τ) ⋅ δq = 0, A(q)δq = 0 dt ∂˙q ∂q

1736-1813

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 49 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Let A(q) = [A1 (q) A2 (q)], and A2 (q) ∈ Rk×k is invertible, then δq1 ∈ Rn−k are free variables: δq2 = −A−1 2 (q)A1 (q)δq1 ⇒(

d ∂L ∂L − − τ) ⋅ δq dt ∂˙q ∂q

=(

d ∂L ∂L d ∂L ∂L − − τ1 ) ⋅ δq1 + ( − − τ2 ) ⋅ δq2 dt ∂˙q1 ∂q1 dt ∂˙q2 ∂q2

=(

d ∂L ∂L d ∂L ∂L − − τ1 ) ⋅ δq1 + ( − − τ2 ) ⋅ (−A−1 2 A1 )δq1 dt ∂˙q1 ∂q1 dt ∂˙q2 ∂q2

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 50 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

As δq1 ∈ Rn−k is free, (

d ∂L ∂L d ∂L ∂L − − τ1 ) − AT1 A−T − − τ2 ) = 0 2 ( dt ∂˙q1 ∂q1 dt ∂˙q2 ∂q2 Lagrange-d’Alembert equation

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 51 ◇ Example: Dynamics of a rolling disk Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Pfaffian constraint: ⎧ ⎪ ⎪ x˙ − ρ cos θ˙ ϕ˙ = 0 ⎨ ⎪ y˙ − ρ sin θ ϕ˙ = 0 ⎪ ⎩

ϕ θ (x, y)

1 0 0 −ρ cos θ ⇒ A(q)˙q = [ 0 1 0 −ρ sin θ ] q˙ = 0 L(q, q˙ ) = 21 m(˙x2 + y˙2 ) + 21 I1 θ¨2 + 21 I2 ϕ˙2 Lagrange-d’Alembert equation: ⎤ ⎡ 0 ⎤ ⎡ m ⎥ ⎢ 0 ⎥⎞ ⎛⎢ m ⎥ ⎥ ⎢ ¨ q−⎢ I1 ⎥ ⎢ τθ ⎥⎠ ⋅ δq = 0 ⎢ ⎝⎢ ⎥ ⎢ ⎥ τ I 2 ⎦ ⎣ ϕ ⎦ ⎣ where A(q) ⋅ δq = 0.

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 52 As Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

{

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

the equation of motion is: 0 0 ] [ x¨ ] + [ I1 0 ] [ θ¨ ] − [ τθ ]) ⋅ [ δθ ] = 0 ([ mρc τϕ y¨ 0 I2 δϕ θ mρsθ ϕ¨ 0 0 ] [ x¨ ] + [ I1 0 ] [ θ¨ ] = [ τθ ] ⇒ [ mρc τϕ y¨ 0 I2 θ mρsθ ϕ¨ As ⎧ ⎪ ⎪ x¨ = ρ cos θ ⋅ ϕ¨ − ρ sin θ ⋅ θ˙ ϕ˙ ⎨ ⎪ y¨ = ρ sin θ ⋅ ϕ¨ + ρ cos θ ⋅ θ˙ ϕ˙ ⎪ ⎩ ¨ I 0 τ ⇒ [ 01 I + mρ2 ] [ θ¨ ] = [ τϕθ ] ϕ 2 Solve for (θ(t), ϕ(t)), and then solve for (x(t), y(t)) from: ⎧ ⎪ ⎪ x˙ = ρ cos θ ⋅ ϕ˙ ⎨ ⇐ 1st order differential equation ⎪ y˙ = ρ sin θ ⋅ ϕ˙ ⎪ ⎩

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 53 ◻ Nature of nonholonomic constraints:

Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Consider q = (r, s) ∈ R2 × R with Pfaffian 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 ∂Lc ∂Lc − = 0, i = 1, 2 dt ∂˙ri ∂ri ⎛ ∂L ∂L ∂aj ⎞ d ∂L ∂L ˙rj = 0 − ai (r) ) − − ⇒ ( ∑ dt ∂˙ri ∂˙s ⎝ ∂ri ∂˙s j ∂ri ⎠

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 54 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

∂aj ⎞ d ∂L ∂L d ∂L ∂L ∂L ⎛ ˙rj (∗) − ) − ai (r) ( − )= a˙ i (r) − ∑ dt ∂˙ri ∂ri dt ∂˙s ∂s ∂˙s ⎝ j ∂ri ⎠ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ≠0 If the constraint is holonomic, i.e.

⇒(

ai (r) =

∂h for some h ∶ E ↦ R ∂ri

then RHS (right hand side) of (∗) equals ∂L ⎛ ∂2 h ∂2 h ⎞ ˙rj − ∑ ˙rj = 0 ∑ ∂˙s ⎝ j ∂ri ∂rj j ∂rj ∂ri ⎠

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 55 ◻ Metric, duality and orthogonality on Tq E: Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

xn

Tq Q x1

q

⋯

1 K = q˙ T M(q)˙q 2 1 = ≪ q˙ , q˙ ≫M 2

Q = h−1 (0)

Tq Q⊥ = {V1 ∈ Tq E∣ ≪ V1 , V2 ≫M = V1T MV2 = 0, ∀V2 ∈ Tq Q}

Tq∗ Q⊥ = {f ∈ Tq∗ E∣⟨f , V⟩ = 0, ∀V ∈ Tq Q} ∶ constraint forces Tq E = Tq Q ⊕ Tq Q⊥ , Tq∗ E = Tq∗ Q ⊕ Tq∗ Q⊥

Definition: M b ∶ Tq E ↦ Tq∗ E, ⟨M ♭ V1 , V2 ⟩ = V1T MV2 =≪ V1 , V2 ≫M M ♯ ∶ Tq∗ E ↦ Tq E, M ♯ = M ♭

−1

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 56 Chapter 4 Robot Dynamics and Control

Reciprocal

Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Property 4: Under the basis

∂ ∂qi

and dqi , i = 1, . . . , n of Tq E and Tq∗ E

respectively, the matrix representation of M ♭ and M ♯ is M and M −1 respectively. Property 5:

M ♯ (Tq∗ Q) = Tq Q

M ♯ (Tq∗ Q⊥ ) = Tq Q⊥

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 57 Given

h ∶E ↦ Rk , m = n − k

Chapter 4 Robot Dynamics and Control

h∗ ≜ Tq h ∶Tq E ↦ Th(q) Rk

∗ h∗ ≜ Tq∗ h ∶Th(q) Rk ↦ Tq∗ E

Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

we have Property 6: ∗ ker h∗ = Tq Q, h∗ (Tq Q⊥ ) = Th(q) Rk , h∗ (Th(q) Rk ) = Tq∗ Q⊥ h∗ Tq∗ E Tq∗ Rk M2♯

M♯ Tq E

h∗

Th(q) Rk

M2♯ = h∗ ○ M ♯ ○ h∗

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 58 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Lemma 1: The map (I − Pω ) ∶ Tq∗ E ↦ Tq∗ Q⊥ given by (I − Pω ) = h∗ ○ M2♭ ○ h∗ ○ M ♯ is a well-defined projection map, with the property: (I − Pω )f1 = 0, ∀f1 ∈ Tq∗ Q (I − Pω )f2 = f2 , ∀f2 ∈ Tq∗ Q⊥

Proof : Given f1 ∈ Tq∗ Q, M ♯ (f1 ) ∈ Tq Q = ker h∗ , then (I − Pω )(f1 ) = 0. For f2 ∈ Tq∗ Q⊥ , ∃λ ∈ Rn−m s.t. f2 = h∗ λ, and (I − Pω )f2 = h∗ M2♭ h∗ M ♯ h∗ λ = h∗ λ = f2 ⇒ Pω ∶ Tq∗ E ↦ Tq∗ Q is a well-defined projection map. Similarly, PT ∶ Tq E ↦ Tq Q, PT = I − M ♯ h∗ M2b h∗

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 59 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

and (I − PT ) ∶ Tq E ↦ Tq Q⊥ are projection maps. Lemma 2: Pω M = MPT Pω h∗ = h∗ PT = 0 PT = PTω

For nonholonomic constraints: h∗ ← A(q)

h∗ ← A∗ (q)

Tq Q ← ∆ q

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

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 60 ◻ Lagrange’s equations of motion: Chapter 4 Robot Dynamics and Control Lagrangian Equations

M(q)¨q + C(q, q˙ ) + N + AT (q)λ = τ

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

Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

˜ = Pω C C ˜ = Pω N N τ˜ = Pω τ ˙ θ˙ = Pω M θ¨ ≜ M ˜ θ¨ ∶ intertia forces in Tq∗ Q M θ¨ + AT (AM −1 AT )−1 A

Definition: Dynamics in Tq∗ Q ˜ +N ˜ θ¨ + C ˜ = τ˜ M

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints (I − Pω )(M¨q + C + N) = (I − Pω )τ + AT τ

Let Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

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

and

Pω M = MPT then the dynamics in T Q : ∗

⊥

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

◻ Geometric Interpretation:

∇↔M M¨q + C + N = τ + AT λ ⇔ M∇q˙ q˙ = τ − N + AT λ ˜ ↔ induced metric on Tq Q ∇

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

↑

tangent vector field normal vector field

61

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 62 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

˜ X Y + S(X, Y) ∇X Y = ∇ −1 M (I − PT )(¨q + M C) = (I − Pω )(τ − N) + AT λ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ S(˙q,˙q)

MS(˙q, q˙ ): centrifugal force due to curvature of Q in E ⎫ ⎪ ⎪ ⎪ ⎬ for hybrid control design T ⎪ MS(˙q, q˙ ) = (I − Pω )(τ − N) + A λ ⎪ ⎪ ⎭ ˜ ˜ q˙ q˙ = τ˜ − N M∇

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 63 ◇ Example: Dynamics of a Spherical Pendulum Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

1 K = m(˙x2 + y˙2 + z˙2 ) 2 1 = q˙ T M˙q 2 q = (x, y, z)T , M = mI

h ∶qT q − r2 = 0

A = (x, y, z), M2♯ = AM ♯ AT = r2 /m ⎡ y2 + z 2 −xy −xz 1 ⎢ Pω = 2 ⎢ −yz x2 + z 2 −yz ⎢ r ⎢ −zx −zy x2 + y2 ⎣ ⎡ x2 xy xz ⎤ ⎥ 1 ⎢ I − Pω = 2 ⎢ yx y2 yz ⎥ ⎢ r ⎢ zx zy z2 ⎥ ⎥ ⎣ ⎦

⎤ ⎥ ⎥ ⎥ ⎥ ⎦

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 64 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

PT = PTω = Pω (µ, ν): Spherical coordinates q = (r cos µ cos ν, r cos µ sin ν, r sin µ)T ˜ q˙ q˙ = PT (∇q˙ q˙ ) ∇ −r sin µ cos ν −r sin ν = [ −r sin µ sin ν r cos ν ] [ vv21 ] r cos µ 0 S(˙q, q˙ ) = (I − PT )(∇q˙ q˙ ) r cos µ cos ν = (− µ˙ 2 − cos2 µ ν˙2 ) [ r cos µ sin ν ] r sin µ where v1 = µ¨ + sin µ cos µ ν˙2 v2 = cos µ ν¨ − 2 sin µ µ˙ ν˙

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 65 Chapter 4 Robot Dynamics and Control

◻ Control Algorithm: 1

Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

holonomic constraints: q˜ ∶ coordinates of Q q = ψ(˜q) ⇒ q˙ = J ⋅ q˙˜

τ = MJ(q¨˜ d − Kv ˙˜e − Kp ˜e) + C1 + N + AT (−λd + KI

∫ (λ − λ )) d

nonholonomic constraints: Let J(q) ∈ Rn×m be s.t. AJ = 0. Write q˙ = J ⋅ u for some u τ = MJ(˙ud − Kp (u − ud )) + M˙Ju + C + N + AT (−λd + KI (λ − λd )) 2

∫

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 66 ◇ Example: 6-DoF manipulator on a sphere with frictionless point contact Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Contact constraint: vz = 0 ⇔ [0 0 1 0 0 0]Adg −1 Vof = 0 fl f

⇒ Holonomic constraint: η = (αoT , αfT , ψ): Parametrization of Q Pω = diag(1, 1, 0, 1, 1, 1) Newton-Euler Equations of motion: M V˙ of − adTVof MVof = Fm + G + AT λ ⎡ Rψ Mo −Mf 0 ⎤ ⎢ ⎥⎡ ˙ 0 0 0 ⎥ ⎢ αo ⎢ Vof = ⎢ Rψ Ro Ko Mo −Ro Kf Mf 0 ⎥ ⎢ α˙ f ⎢ ⎥ ⎢ ψ˙ ⎢ −To Mo ⎣ −Tf Mf 1 ⎥ ⎣ ⎦ MJ η¨ + C1 = Fm + G + AT λ ∗

⎤ ⎥ ⎥ ≜ J η˙ ⎥ ⎦

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 67 Pω (∗) ∶ Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

˜ 1 = B 1 Fm + B 1 G ˜ η¨ + C M −ϕ3 − λ = b2 Fm + b2 G ˜ 1 − B1 G ˜ η¨ d − Kv ˙˜e − Kp ˜e) + C Fˆ = [f1 f2 f4 f5 f6 ]T = M( f3 = −ϕ3 − λd + KI τ = JsT Fm

∫ (λ − λ ) − b G d

2

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 68 ◇ Example: 6-DoF manipulator rolling on a sphere Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator

⎡ 1 0 0 0 0 0 ⎤ ⎢ 0 1 0 0 0 0 ⎥ ⎢ ⎥ ⎢ 0 0 1 0 0 0 ⎥ Vlo lf = 0 ⎢ 0 0 0 0 0 1 ⎥ ⎣ ⎦ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ˙ 1 (q) A T A1 λ, λ ∈ R4

fc = ωx [ ωy ] = −R0 (Kf + Rψ Ko Rψ )Mf α˙ f Vof = Adgflf ⋅ Vlo lf

Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

= Adgflf

⎡ 0 ⎢ 0 ⎢ ⎢ 0 ⎢ ⎢ −Ro (Kf + Rψ Ko Rψ )Mf ⎢ o ⎣

≜ Jf α˙ f span{Jf }: Not involutive

⎤ ⎥ ⎥ ⎥ α˙ f ⎥ ⎥ ⎥ ⎦

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 69 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body

MJf α¨ f + (M˙Jf α˙ f − adTJf α˙ f MJf α˙ f ) = Fm + G + AT λ Fm = MJf (α¨ fd − Kp (α˙ f − α˙ fd )) + (M˙Jf α˙ f − adTJ α˙ MJf α˙ f ) f

+ AT (−λd +

Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

∫ (λ − λ )) − G d

◇ Example: Redundant parallel manipulator θ = (θ 1 , . . . , θ 6 ) ∈ E

Dynamics of an Open-chain Manipulator Newton-Euler Equations

f

⎡ ⎢ ⎢ H(θ) = ⎢ ⎢ ⎢ ⎣

θ a = (θ 1 , θ 3 , θ 5 )

θ p = (θ 2 , θ 4 , θ 6 ) xa + lc1 + lc12 − xb − lc3 − lc34 ya + ls1 + ls12 − yb − ls3 − ls34 xa + lc1 + lc12 − xc − lc5 − lc56 ya + ls1 + ls12 − yc − ls5 − ls56

⎤ ⎥ ⎥ ⎥=0 ⎥ ⎥ ⎦

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 70 Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

Mi (θ) ∈ R2×2 : ith chain M(θ) = diag(M1 (θ), . . . , M3 (θ)) M(θ)θ¨ + C + N = τ + AT λ If all joints are actuated, Position control of end-effector + internal grasping force As τ2 , τ4 , τ6 = 0, θ˜ ∈ R2 ∶ local parametrization of Q = H −1 (0) ˜ ∶embedding of Q in E θ = ψ(θ) θ˙ = J θ˙˜

Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints 71 Given Pω ∶ Tθ∗ E ↦ Tθ∗ Q, the dynamics in Tθ∗ Q is given by: Chapter 4 Robot Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Dynamics of an Open-chain Manipulator Newton-Euler Equations Coordinateinvariant algorithms for robot dynamics Lagrange’s Equations with Constraints

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

P˜ ω = (P1 , P3 , P5 ) τˆ = Pˆ ω τ˜ = Pω τ ∈ R6

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