Chapter 4 Robot Dynamics and Control

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

1

Chapter 4
Lecture Notes
Robot
Dynamics and
Control
for
Lagrangian A Mathematical Introduction to
Equations

Inertial Robotic Manipulation

Properties of
Rigid Body

Dynamics of
an Open-chain By
Z.X. Li∗ and Y.Q. Wu#
Manipulator

Newton-Euler
Equations
∗
Coordinate- Dept. of ECE, Hong Kong University of Science & Technology
invariant #
algorithms for
School of ME, Shanghai Jiaotong University
robot
dynamics

Lagrange’s
24 July 2009
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

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

2

Chapter 4
Robot
Dynamics and
Chapter 4 Robot Dynamics and Control
Control
1 Lagrangian Equations
Lagrangian
Equations

Inertial 2 Inertial Properties of Rigid Body

Properties of
Rigid Body

Dynamics of
3 Dynamics of an Open-chain Manipulator
an Open-chain
Manipulator

Newton-Euler
4 Newton-Euler Equations
Equations

Coordinate- 5 Coordinate-invariant algorithms for robot dynamics

invariant
algorithms for
robot
dynamics 6 Lagrange’s Equations with Constraints
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.1 Lagrangian Equations

y
3
◻ A Simple Example:
Fy F
Chapter 4
m Fx
Robot
Dynamics and
Control
mg
Lagrangian ♢ Review: x
Equations Newton’s Equation: Lagrangian Equation:
Inertial
d ∂L ∂L
Properties of
− = Fx
Rigid Body mẍ = Fx dt ∂ẋ ∂x
Dynamics of
mÿ = Fy − mg
⇔ d ∂L ∂L
an Open-chain
Manipulator − = Fy
dt ∂ẏ ∂y
Newton-Euler Momentum: Px = mẋ
Equations Lagrangian function:
Py = mẏ
Coordinate-
d d
L = T − V, Px = ∂L
∂ẋ , Py =
∂L
∂ẏ
dt Px = Fx , dt Py = Fy − mg
invariant
algorithms for Kinetic energy:
robot
dynamics T = 21 m(ẋ2 + ẏ2 )2
Lagrange’s
Equations with
Potential energy:
Constraints V = mgy
 Chapter 4 Robot Dynamics and Control

4.1 Lagrangian Equations

4
◻ Generalization to multibody systems:
Chapter 4 qi , i = 1, . . . , n: generalized coordinates
Robot
Dynamics and
y Kinetic energy:
Control
m3
q3 T = T(q, q̇)
Lagrangian
Equations Potential energy:
Inertial V = V(q)
Properties of m2
Rigid Body Lagrangian:
Dynamics of q2 L(q, q̇) = T(q, q̇) − V(q)
an Open-chain
Manipulator
m1
τi , i = 1, . . . , n: external force on qi
Newton-Euler Lagrangian Equation:
Equations
q1
Coordinate- x
invariant d ∂L ∂L
algorithms for − = τi , i = 1, . . . , n
robot dt ∂q̇i ∂qi
dynamics

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

4.1 Lagrangian Equations

5
◇ Example: Pendulum equation
Generalized coordinate: y
Chapter 4 1
Robot θ∈S
Dynamics and
Control Kinematics: x
Lagrangian x = l sin θ, y = −l cos θ
⇒ ẋ = l cos θ ⋅ θ̇, ẏ = l sin θ ⋅ θ̇
Equations

Inertial
Properties of Kinetic energy: θ
T(θ, θ̇) = m(ẋ + ẏ ) = ml θ̇
Rigid Body
1 2 2 1 2 2
Dynamics of
an Open-chain 2 2
Manipulator Potential energy:
Newton-Euler
Equations
V = mgl(1 − cos θ) mg
Coordinate-
Lagrangian function:
L = T − V = ml2 θ̇ − mgl(1 − cos θ), ⇒ = −mgl sin θ
invariant 1 ∂L ∂L
algorithms for = ml2 θ̇,
robot
dynamics
2 ∂ θ̇ ∂θ
Equation of motion:
− = τ ⇒ ml2 θ̈ + mgl sin θ = τ
Lagrange’s
Equations with d ∂L ∂L
Constraints
dt ˙ ∂θ
 Chapter 4 Robot Dynamics and Control

4.1 Lagrangian Equations

6
◇ Example: Dynamics of a Spherical Pendulum
Chapter 4

⎡ l sin θ cos ϕ ⎤
Robot

⎥
Dynamics and

r(θ, ϕ) = ⎢ l sin θ sin ϕ ⎥

Control
⎢
⎢ −l cos θ ⎥
⎣ ⎦
Lagrangian
Equations

T = m∥ṙ∥2 = ml2 (θ̇ 2 + (1 − cos2 θ)ϕ̇2 )

Inertial 1 1
Properties of
Rigid Body 2 2 θ
Dynamics of
an Open-chain
V = −mgl cos θ

L(q, q̇) = ml2 (θ̇ + (1 − cosθ )ϕ̇2 ) + mgl cos θ

Manipulator
1
Newton-Euler
Equations 2
Coordinate-
invariant ϕ mg
algorithms for
robot
dynamics

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

4.1 Lagrangian Equations

7

⎧
⎪
⎪
Chapter 4

⎪ = (ml2 θ̇) = ml2 θ̈, = ml2 sin θ cos θ ϕ̇2 − mgl sin θ
d ∂L d ∂L
⎪
Robot

⎪
⎪ dt ∂ θ̇ dt
Dynamics and

⎨
Control ∂θ
⎪
⎪
⎪ = (ml2 sin2 θ ϕ̇) = ml2 sin2 θ ϕ̈ + 2ml2 sin θ cos θ θ̇ ϕ̇,
d ∂L d ∂L
⎪
Lagrangian

⎪
⎪
=0
Equations

Inertial ⎩ dt ∂ ϕ̇ dt ∂ϕ
Properties of

θ̈ ] + [ −ml2 sθ cθ ϕ̇2 ] + [ mglsθ ] = [ 0 ]

Rigid Body

[ 2 2 ][
Dynamics of
ml2 0
an Open-chain 0 ml sθ ϕ̈ 2ml2 sθ cθ θ̇ ϕ̇ 0 0
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
 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 r
Control B
Lagrangian
Equations
ra
Inertial
Properties of
Rigid Body A gab
Dynamics of
an Open-chain
Manipulator

Newton-Euler
Equations
Volume occupied by the body: V
Coordinate-
Mass density: ρ(r)
invariant
algorithms for Mass: m= ∫
V
ρ(r)dV

r≜
robot
1
dynamics

Lagrange’s
Mass center:
m V ∫
ρ(r)rdV
Equations with Relative to frame at the mass center: r=0
Constraints
 Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body

9
In A-frame
Chapter 4

ρ(r)∥ṗ + Ṙr∥2 dV = ρ(r)(∥ṗ∥2 + 2ṗT Ṙr + ∥Ṙr∥2 )dV

Robot
1 1
Dynamics and
Control T=
2 V ∫ ∫ 2 V
= m∥ṗ∥2 + ṗT Ṙ ρ(r)rdV +
Lagrangian
1 1
Equations
∫ ∫ ρ(r)∥Ṙr∥2 dV
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
Inertial 2 V 2
=0
Properties of
Rigid Body

Dynamics of
an Open-chain
Manipulator
1
Newton-Euler
Equations 2 ∫ V
ρ(r)∥Ṙr∥2 dV
1 1 1
∫ ρ(r)∥R Ṙr∥ dV = 2 ∫ ρ(r)∥ω̂r∥ dV = 2 ∫ ρ(r)∥r̂ω∥ dV
Coordinate- T 2 2 2
invariant =
algorithms for 2

∫ ρ(r)(−ω r̂ ω)dV = 2 ω ( ∫ (−ρ(r)r̂) dV) ω ≜ 2 ω I ω

robot
dynamics 1 1 T 2 1 T 2 T
=
Lagrange’s 2
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body

10

⎤
where
⎥
⎡ Ixx Ixy Ixz
⎥
⎢
Chapter 4
I =− ∫ ρ(r)r̂2 dV ≜ ⎢
⎢ Ixy Iyy Iyz ⎥
⎥
Robot

⎣ ⎦
Dynamics and ⎢ Ixz Iyz Izz
Control
with
∫ ρ(r)(y + z2 )dxdydz, Ixy = − ∫ ρ(r)xydxdydz
Lagrangian
2
Equations Ixx =
Inertial
Properties of
Rigid Body

T = m∥ṗ∥2 + (ωb )T I b ωb = m∥RT ṗ∥2 + (ωb )T I b ωb

1 1 1 1
Dynamics of
an Open-chain
Manipulator
2 2 2 2
= (V ) [ ] Vb
1 b T mI 0
Newton-Euler
2 0 Ib
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹¶
Equations

Coordinate-
invariant Mb
algorithms for

For V̂ b = g −1 ⋅ ġ, with ω̂b = RT ⋅ Ṙ and vb = RT ṗ, M b is the

robot
dynamics

Lagrange’s
Equations with
Generalized inertia matrix in B frame.
Constraints
 Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body

11
◇ Example: M b for a rectangular object
m z
Chapter 4
Robot ρ=
lωh
ρ(y2 + z2 )dxdydz
Dynamics and
Control

Lagrangian
Ixx = ∫ V y
Equations h ω l
x
∫ ∫ ∫ (y2 + z2 )dxdydz
2 2 2
Inertial =ρ h l
Properties of
Rigid Body
− h2 − ω2 − 2l

= ρ( (lω3 h + lωh3 )) = (ω2 + h2 )

Dynamics of 1 m w
an Open-chain
Manipulator 12 12
h ω l

Ixy = − ∫ ρxydv = −ρ ∫ ∫ ∫
Newton-Euler 2 2 2
xydxdydz
− h2 − ω2 − 2l
Equations
V
Coordinate-
invariant l
h ω

= −ρ x ∣ dydz = 0
algorithms for y 2 2
robot
dynamics ∫ ∫ 2

− h2 − ω2
2

2 −l
Lagrange’s 2
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body

12
⎤
12 (w +h ) ⎥
⎡ m 2 2
⎥
⎢ 0 0
12 (l + h2 ) ⎥ , M = [ mI03×3 I0 ]
⎢
2 ⎥
m 2
I =⎢ 0 0
⎥
12 (w + l ) ⎦
⎢
⎣
⎢ m 2
Chapter 4 0 0
Robot
Dynamics and
g2 (t)
Control
g0
Lagrangian
Equations
V̂1 = g1−1 ⋅ ġ1 , M1 A B
1
Inertial
Properties of T = V1T M1 V1
Rigid Body 2 g1 (t) g0
Dynamics of V1 = Adg0 V2
an Open-chain
Manipulator

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

Newton-Euler
Equations
1 1 1
Coordinate- 2 2 2
invariant
algorithms for ◻ M under change of frames:
robot
dynamics

Lagrange’s M2 = AdTg0 M1 Adg0

Equations with
Constraints
 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 ⎡ Ixxi 0 0
⎥ , i = 1, 2
Dynamics and ⎢
⎥
Ii = ⎢ 0 Iyyi 0
⎣ ⎦
Control
⎢ 0 0 Izzi
Lagrangian
Equations

Inertial

T(θ, θ̇) = m1 ∥v1 ∥2 + ωT1 I1 ω1

Properties of l2
Rigid Body 1 1
Dynamics of 2 2 r 2 θ2

+ m2 ∥v2 ∥ + ωT2 I2 ω2
an Open-chain
Manipulator
1 2 1
l1
Newton-Euler 2 2 y
Equations r1
θ1
Coordinate- x
invariant
0 0
ω1 = [ 0 ] ω2 = [ ]
algorithms for
0
θ̇ 1 + θ̇ 2
robot
dynamics
θ̇ 1
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.2 Intertial Properties of Rigid Body

14
xi
Pi = [ yi ]: Mass center γi : Distance from joint i to mass center
0
ẋ1 = −r1 s1 θ̇ 1
Chapter 4
Robot x1 = r1 c1
Dynamics and ⇒
Control
y1 = r1 s1 ẏ1 = r1 c1 θ̇ 1
x2 = l1 c1 + r2 c12 ẋ2 = −(l1 s1 + r2 s12 )θ̇ 1 − r2 s12 θ̇ 2
Lagrangian
Equations

y2 = l1 s1 + r2 s12
⇒
ẏ1 = (l1 c1 + r2 c12 )θ̇ 1 + r2 c12 θ̇ 2
Inertial
Properties of

T(θ, θ̇) = m1 (ẋ1 + ẏ1 ) + Iz1 θ̇ 12 + m2 (ẋ2 + ẏ2 ) + Iz2 (θ̇ 12 + θ̇ 22 )

Rigid Body
1 2 2 1 1 2 2 1
Dynamics of
an Open-chain 2 2 2 2
α + 2βc δ + βc2
= [θ̇ 1 θ̇ 2 ] [ δ + βc 2 ] [ θ̇ 1 ]
Manipulator
1
Newton-Euler
2 δ θ̇ 2
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
Equations 2
Coordinate-
M(θ)
Iz1 + Iz2 + m1 r12 + m2 (l12 + r22 ), β
= m2 l1 r2 , δ = Iz2 + m2 r22 , L = T
invariant
algorithms for
α=
−βs2 θ̇ 2 −βx
robot

⇒ M(θ) [ θ̈ 1 ] + [ ] [ θ̇ 1 ] = [ ττ21 ]
dynamics

Lagrange’s
Equations with θ̈ 2 βs2 θ̇ 1 0 θ̇ 2
Constraints
 Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator

15
◻ Dynamics of open-chain manipulator:
Definition:
Li : frame at mass center of link i, gsli (θ) = expξ̂1 θ 1 ⋯ expξ̂i θ i gsli (o)
Chapter 4
Robot
Dynamics and
Control θ1
Lagrangian θ2 θ3
Equations l1 l2
L2 L3
Inertial
Properties of
Rigid Body r1 r2
l0
Dynamics of L1
an Open-chain
Manipulator r0
S
Newton-Euler

⎡ ⎤
Equations

⎢ θ̇ 1 ⎥
⎢ ⋮ ⎥
Coordinate-

⎢ ⎥
invariant

⎢ ⎥
algorithms for

Vsli = Jsli (θ)θ̇ = [ξ1 ξ2 ⋯ ξi 0 ⋯ 0] ⎢ θ̇ i ⎥ = Ji (θ)θ̇

robot

⎢ ⎥
b b † † †

⎢ ⎥
dynamics
θ̇ i+1
⎢ ⋮ ⎥
⎢ ⎥
Lagrange’s

⎣ ⎦
Equations with
Constraints θ̇ n
 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

Ti (θ, θ̇) = (Vslbi )T Mib Vslbi = θ̇ T JiT (θ)Mib Ji (θ)θ̇

Chapter 4 1 1
Robot
Dynamics and 2 2
n
T(θ) = ∑ Ti (θ, θ̇) = θ̇ T M(θ)θ̇,
Control
1
Lagrangian
Equations i=1 2
1 n
M(θ) = ∑ JiT (θ)Mib Ji (θ) = ∑ Mij (θ)θ̇ i θ̇ j
Inertial
Properties of
Rigid Body
i 2 i,j=1
hi (θ): Height of Li , Vi (θ) = mi ghi (θ), V(θ) = ∑ mi ghi (θ)
Dynamics of
an Open-chain
Manipulator
i=1
Newton-Euler
Equations
Lagrange’s Equation:

−
Coordinate- d ∂L ∂L
invariant = Γi , i = 1, . . . , n,
algorithms for dt ∂ θ̇ i ∂θ i
d ⎛n ⎞ n
robot

∑ Mij θ̇ j = ∑ Mij θ̈ j + Ṁij θ̇ j

dynamics
d ∂L
dt ∂ θ̇ i dt ⎝ j=1 ⎠ j=1
Lagrange’s =
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator

17
∂L 1 n ∂Mkj ∂Mij
θ̇ k θ̇ j −
∂V
= ∑ Ṁij = ∑ θ̇ k
∂θ i 2 j,k=1 ∂θ i ∂θ i k ∂θ k
Chapter 4
Robot n n ∂Mij 1 ∂Mkj
⇒ ∑ Mij θ̈ j + ∑ ( θ̇ j θ̇ k − θ̇ k θ̇ j ) +
Dynamics and ∂V
Control = Γi
j=1 j,k=1 ∂θ k 2 ∂θ i ∂θ i
Lagrangian
Equations n n
⇒ ∑ Mij θ̈ j + ∑ Γijk θ̇ k θ̇ j +
∂V
Inertial = Γi
Properties of
Rigid Body j=1 j,k=1 ∂θ i
1 ∂Mij ∂Mik ∂Mkj
( + − )
Dynamics of
an Open-chain Γijk =
Manipulator
2 ∂θ k ∂θ j ∂θ i
Newton-Euler

θ̇ i ⋅ θ̇ j , i ≠ j : Coriolis force
Equations

Coordinate- θ̇ i2 : Centrifugal force

n
1 n ∂Mij ∂Mik ∂Mkj
Define: cij (θ, θ̇) = ∑ Γijk θ̇ k = ∑ ( + − )θ̇ k
invariant
algorithms for
robot
dynamics k=1 2 k=1 ∂θ k ∂θ j ∂θ i
Lagrange’s
Equations with ⇒ M(θ)θ̈ + C(θ, θ̇)θ̇ + N(θ) = τ
Constraints
 Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator

18

Property 1:
M(θ) = M T (θ), θ̇ T M(θ)θ̇ ≥ 0, θ̇ T M(θ)θ̇ = 0 ⇔ θ = 0
Chapter 4
Robot 1

Ṁ − 2C ∈ Rn×n is skew symmetric

Dynamics and
Control
2
Lagrangian
Equations
Proof :
Inertial

(Ṁ − 2C)ij = Ṁij − 2cij (θ)

Properties of
Rigid Body

Dynamics of n ∂M ∂Mij ∂Mkj

θ̇ k − θ̇ k − θ̇ k +
∂Mik
=∑
an Open-chain ij
Manipulator θ̇ k
Newton-Euler k=1 ∂θ k ∂θ k ∂θ j ∂θ i
n ∂Mkj
θ̇ k −
Equations
∂Mik
Coordinate- =∑ θ̇ k
invariant
k=1 ∂θ i ∂θ j
algorithms for

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

robot
dynamics

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

4.3 Dynamics of Open-chain Manipulator

19
◇ Example: Planar 2-DoF Robot (continued)
Chapter 4 m11 (θ) = α + 2β cos θ 2 , m22 = δ
m12 (θ) = m21 (θ) = δ + β cos θ 2
Robot
Dynamics and
Control

Lagrangian
Equations
c11 (θ, θ̇) = −β sin θ 2 ⋅ θ̇ 2 , c12 (θ, θ̇) = −β sin θ 2 (θ̇ 1 + θ̇ 2 )
Inertial c21 (θ, θ̇) = β sin θ 2 ⋅ θ̇ 1 , c22 (θ, θ̇) = 0
Properties of
⎧
⎪
⎪ ( + − )=
1 ∂M11 ∂M11 ∂M11 1 ∂M11
⎪
⎪
Rigid Body
Γ111 =
⎪
=0
⎪
⎪
⎪
2 ∂θ 1 ∂θ 1 ∂θ 1 2 ∂θ 1
⎪
Dynamics of
⎪
⎪
⎪
an Open-chain
⎪ ( + − )= = −β sin θ 2
1 ∂M11 ∂M12 ∂M21 1 ∂M11
⎪
⎪
Manipulator
Γ2 =
⎪ 11
⎨
Newton-Euler 2 ∂θ 2 ∂θ 1 ∂θ 1 2 ∂θ 2
⎪
⎪
⎪ Γ12
Equations

⎪ ( + − )= = −β sin θ 2
1 ∂M12 ∂M11 ∂M12 1 ∂M11
⎪
⎪
1
=
⎪
⎪
Coordinate-

⎪
2 ∂θ 1 ∂θ 2 ∂θ 1 2 ∂θ 2
⎪
⎪
invariant

⎪
⎪
⎪
algorithms for

( + − )= − = −β sin θ 2
1 ∂M12 ∂M12 ∂M22 ∂M12 1 ∂M22
⎪
⎪
robot 2
⎩
dynamics Γ12 =
2 ∂θ 2 ∂θ 2 ∂θ 1 ∂θ 2 2 ∂θ 1
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator

20
⎧
⎪
⎪ = ( + − )= −
1 ∂M21 ∂M21 ∂M11 ∂M21 1 ∂M11
⎪
⎪
1
⎪
Γ21 = β sin θ 2
⎪
⎪
⎪
2 ∂θ 1 ∂θ 1 ∂θ 2 ∂θ 1 2 ∂θ 2
⎪
⎪
⎪ 2
⎪
Chapter 4

⎪ = ( + − )=
1 ∂M21 ∂M22 ∂M21 1 ∂M22
⎪
Robot
Dynamics and
⎪
⎪ 21
Γ =0
⎨
Control 2 ∂θ 2 ∂θ 1 ∂θ 2 2 ∂θ 1
⎪
⎪
⎪
⎪ = ( + − )=
1 ∂M22 ∂M21 ∂M12 1 ∂M22
⎪
Lagrangian

⎪
1
=0
⎪
Γ22
⎪
Equations

⎪
2 ∂θ 1 ∂θ 2 ∂θ 2 2 ∂θ 1
⎪
⎪
⎪
⎪
Inertial

⎪ = ( + − )=
1 ∂M22 ∂M22 ∂M22 1 ∂M22
⎪
⎪
Properties of 2
⎩
Rigid Body Γ22 =0
2 ∂θ 2 ∂θ 2 ∂θ 2 2 ∂θ 2
−2β sin θ 2 ⋅ θ̇ 2 −β sin θ 2 ⋅ θ̇ 2
Dynamics of
an Open-chain
Ṁ − 2C = [ ]
Manipulator
−β sin θ 2 ⋅ θ̇ 2 0
−2β sin θ 2 ⋅ θ̇ 2 −2β sin θ 2 (θ̇ 1 + θ̇ 2 )
Newton-Euler

−[ ]
Equations

Coordinate- 2β sin θ 2 ⋅ θ̇ 1 0
invariant

β sin θ 2 (2θ̇ 1 + θ̇ 2 )
=[ ] ⇐ skew-symmetric
algorithms for
0
−β sin θ 2 (2θ̇ 1 + θ̇ 2 )
robot
dynamics
0
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator

21
◇ Example: Dynamics of a 3-dof robot
Chapter 4 θ1
Robot
Dynamics and θ2 θ3

⎡ ⎤ ⎡ 0 ⎤ ⎡ 0 ⎤
l1 l2
⎢ ⎥ ⎢ −l0 ⎥ ⎢ −l0 ⎥
Control 0
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
L2 L3

⎢ ⎥ ⎢ 0 ⎥ ⎢ l1 ⎥
0
Equations ξ 1 = ⎢ ⎥ , ξ 2 = ⎢ −1 ⎥ , ξ 3 = ⎢ −1 ⎥
Lagrangian 0
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
r1 r2
⎢ ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥
0
⎢ ⎥ ⎢ 1 ⎥ ⎢ ⎥
0 l0
⎣ ⎦ ⎣ ⎦ ⎣ 1 ⎦
Inertial L1
1
Properties of r0
Rigid Body S

Dynamics of
an Open-chain
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
⎢ I ( 0 ) ⎥ ⎢ ( r1 ) ⎥ ⎢ ( l 1 + r2 ) ⎥
0 0 0
gsl1 (0) = ⎢ ⎥, gsl (0) = ⎢ I ⎥, gsl (0) = ⎢ I ⎥
Manipulator
⎢ ⎥ 2 ⎢ ⎥ 3 ⎢ ⎥
⎢ 0 r0 ⎥ ⎢ 0 l0 ⎥ ⎢ 0 l0 ⎥
⎣ ⎦ ⎣ ⎦ ⎣ ⎦
Newton-Euler
Equations
1 1 1
Coordinate-
invariant
algorithms for
robot
dynamics

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

4.3 Dynamics of Open-chain Manipulator

22
⎡ mi 0 0 ⎤
⎢ 0 mi 0 ⎥
⎢ ⎥
⎢ 0 0 mi ⎥
0
Mi = ⎢ ⎢ 0 ⎥ ⎥
Chapter 4

⎢ 0 Iy i 0 ⎥
Robot Ixi 0
⎢ ⎥
⎢ 0 Iz i ⎥
Dynamics and
0
⎣ ⎦
Control

Lagrangian
0
Equations mi : The mass of the object
Inertial Ixi : The moment of inertia about the x axis
Properties of

Γ112 = (Iy2 − Iz2 − m2 r12 )c2 s2 + (Iy2 − Iz3 )c23 s23 − m3 (l1 c2 + r2 c23 )(l1 s2 + r2 s23 )
Rigid Body

Γ113 = (Iy3 − Iz3 )c23 s23 − m3 r2 s23 (l1 c2 + r2 c23 )

Dynamics of
an Open-chain

= (Iy2 − Iz2 − m2 r12 )c2 s2 + (Iy3 − Iz3 )c23 s23 − m3 (l1 c2 + r2 c23 )(l1 s2 + r2 s23 )
Manipulator
1
Γ12
Γ13 = (Iy3 − Iz3 )c23 s23 − m3 r2 s23 (l1 c2 + r2 c23 )
Newton-Euler 1

= (Iz2 − Iy2 + m2 r12 )c2 s2 + (Iz3 − Iy3 )c23 s23 + m3 (l1 c2 + r2 c23 )(l1 s2 + r2 s23 )
Equations
1
Γ21
Γ22 = −l1 m3 r2 s3 , Γ23 = −l1 m3 r2 s3 , Γ23 = −l1 m3 r2 s3
Coordinate-
invariant 3 2 3

Γ311 = (Iz3 − Iy3 )c23 s23 + m3 r2 s23 (l1 c2 + r2 c23 ) , Γ32

algorithms for 2
robot = l 1 m3 r2 s 3
dynamics

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

4.3 Dynamics of Open-chain Manipulator

23
, V(θ) = m1 gh1 (θ) + m2 gh2 (θ) + m2 gh3 (θ)
∂V
N(θ, θ̇) =
∂θ
Chapter 4

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

Robot
Dynamics and
Control

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

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

Inertial

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

Properties of
Rigid Body

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

Dynamics of
an Open-chain

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

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

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

4.3 Dynamics of Open-chain Manipulator

24
⎡ 0 0 0 ⎤
⎢ 0 0 0 ⎥
⎢ ⎥
⎢ 0 0 0 ⎥
J1 = Jsl1 (θ) = ⎢ 0 0 0 ⎥
⎢ ⎥
Chapter 4 b

⎢ 0 0 0 ⎥
Robot

⎢ 1 0 0 ⎥
Dynamics and

⎣ ⎦
Control

⎡ −r1 c2 0 0 ⎤
Lagrangian

⎢ 0 0 ⎥
⎢ ⎥
Equations

⎢ −r1 0 ⎥
0
J2 = Jslb2 (θ) = ⎢ −1 0 ⎥
Inertial
0
⎢ ⎥
Properties of

⎢ −s2 0 0 ⎥
Rigid Body 0
⎢ 0 0 ⎥
⎣ ⎦
Dynamics of
an Open-chain c2
⎡ −l2 c2 − r2 c23 0 ⎤
⎢ 0 ⎥
Manipulator
0
⎢ ⎥
⎢ −r2 − l1 c3 −r2 ⎥
Newton-Euler
0 l1 s3
J3 = Jsl3 (θ) = ⎢ −1 ⎥
Equations

−1
b 0
⎢ ⎥
⎢ 0 ⎥
0
−s23
Coordinate-

⎢ ⎥
invariant

⎣ 0 ⎦
algorithms for
0
robot c23 0
dynamics

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

4.3 Dynamics of Open-chain Manipulator

25

M11 M12 M13

Chapter 4 M(θ) = [ M21 M22 M23 ] = J1T M1 J1 + J2T M2 J2 + J3T M3 J3
Robot M31 M32 M33
M11 = Iy2 s2 + Iy3 s223 + Iz1 + Iz2 c22 + Iz3 c223 + m2 r12 c22 + m3 (l1 c2 + r2 c23 )2
Dynamics and
Control 2
Lagrangian
Equations M12 = M13 = M21 = M31 = 0
M22 = Ix2 + Ix3 + m2 l12 + M2 r12 + m3 r22 + 2m3 l1 r2 c3
Inertial
Properties of

M23 = Ix3 + m3 r22 + m3 l1 r2 c3

Rigid Body

Dynamics of

M32 = Ix3 + m3 r22 + m3 l1 r2 c3

an Open-chain
Manipulator

M33 = Ix3 + m3 r22

Newton-Euler
Equations

Coordinate-
n
invariant
1 n ∂Mij ∂Mik ∂Mkj
Cij (θ, θ̇) = ∑ Γijk θ̇ k = ∑( + − ) θ̇ k
algorithms for
robot
dynamics
k=1 2 k=1 ∂θ k ∂θ j ∂θ i
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.3 Dynamics of Open-chain Manipulator

26

◻ Additional Properties of the dynamics in

Chapter 4
Robot terms of POE:
Dynamics and
Control Define:
⎧
⎪Ad−1ξ̂ j+1 θ j+1 ξ̂ θ i > j
Lagrangian

⎪
⎪
⎪
Equations

⎪ e ⋯e i i
Aij = ⎨I
Inertial

⎪
⎪
i=j
Properties of

⎪
⎪
Rigid Body

Dynamics of ⎪
⎩0 i<j
Ji (θ) = Adg −1 (0) [Ai1 ξ1 ⋯Aii ξi 0⋯0]
an Open-chain
Manipulator

Newton-Euler sl i

Mi′ ( intertia of ith link in S)

Equations

Coordinate-
= AdTg−1 Mi Adg −1
sl (0)
i sl (0)
i
invariant
algorithms for
robot
dynamics

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

4.3 Dynamics of Open-chain Manipulator

27

Property 2:
n
Mij (θ) = ∑ ξiT ATli Ml′ Alj ξj
Chapter 4
Robot
Dynamics and
Control l=max(i,j)

1 n ∂Mij ∂Mik ∂Mkj

Cij (θ, θ̇) = ∑( + − ) θ̇ k
Lagrangian
Equations

Inertial 2 k=1 ∂θ k ∂θ j ∂θ i
Properties of
Rigid Body where
n
= ∑ ([Ak−1,i ξi , ξk ]T ATlk Ml′ Alj ξj
∂Mij
Dynamics of
an Open-chain
Manipulator
∂θ k l=max(i,j)
Newton-Euler
Equations
+ξiT ATli Ml′ Alk [Ak−1,j ξj , ξk ])
Coordinate-
invariant
algorithms for
robot
dynamics

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

4.4 Newton-Euler Equations

28
◻ Newton-Euler equations in body frame:
Newtons Equation:
Chapter 4
Robot

(mṗ) = mp̈
Dynamics and d
Control f =
Lagrangian
dt
Equations
Spatial angular momentum:
Inertial

I′ ⋅ ωs = R(I ⋅ ωb ) = R ⋅ I ⋅ RT ⋅ωs
Properties of

´¹¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹¶
Rigid Body

Dynamics of
an Open-chain I′
Manipulator

Newton-Euler
Equations r
Coordinate-
T
invariant
algorithms for
robot
dynamics
f
Lagrange’s S
g ∶ (R, p)
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.4 Newton-Euler Equations

29

(I ω ) = (RI b RT ωs ) = I s ω̇s + ṘI b RT ωs + RI b ṘT ωs

d s s d
Chapter 4
τ=
dt dt
= I ω̇ + ṘR I ω − RI b RT ω̂s ωs = I s ω̇s + ωs × (I s ωs )
Robot
Dynamics and s s T s s
±̂
Control

Lagrangian s ω
Equations

Inertial ◻ Transformation of all equations to

Properties of
Rigid Body twist/wrench in body frame:
(mṗ) = (mRvb ) = mṘvb + mRv̇b , RT f s = mRT Ṙvb + mv̇b
Dynamics of
d d
an Open-chain
dt dt
⇒ f = mωb × v̇b + mv̇b ,
Manipulator
b
Newton-Euler
Equations

τb = RT τs = RT (RI b ωb ) = I b ω̇b + ωb × I b ωb
d
Coordinate-
invariant dt
] + [ ωb × mv
algorithms for

⇒[ b ][ ] = [ f b ] = F b (∗)
mI 0 b b b b
robot v̇
ω ×I ω
dynamics b b b
0 I ω̇ τ
Lagrange’s
Equations with
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹¶ ´¹¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹¶
Constraints
M Vb
 Chapter 4 Robot Dynamics and Control

4.4 Newton-Euler Equations

30

Chapter 4 Define: se(3) × se(3) ↦ se(3), ( ξ̂1 , ξ̂2 ) ↦ ξ̂1 ξ̂2 − ξ̂2 ξ̂1 ≜ ξ̂, if
ξ̂i = [ ω̂0i v0i ] , i = 1, 2
Robot
Dynamics and
Control
then
ξ̂ = [ (ω1 ×0ω2 ) ω̂1 v2 −
∧
Lagrangian
Equations ω̂2 v1 ] = ad ⋅ ξ
0 ξi 2
Inertial
where
ad ξ1 = [ ω̂01 ω̂ v̂1 ]
Properties of
Rigid Body

Dynamics of 1
an Open-chain Thus
(∗) ⇔ Mb V̇b − adV b Mb Vb = F b
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

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

4.4 Newton-Euler Equations

31
◻ Coordinate invariance of Newton-Euler
equations:
Chapter 4
Robot
Dynamics and r g0
Control
B
M1 V̇1 − adTV1 M1 V1 = F1
g2
Lagrangian
Equations
V1 = Adg0 V2
AdTg0 F1 = F2 ⇒
Inertial
Properties of

F1 = (AdTg0 )−1 F2 = (Ad−1g 0 ) F2

Rigid Body T A g1
Dynamics of

M1 = Ad−T −1
an Open-chain
g0 M2 Adg0
Manipulator

−T −1 −T −1
Newton-Euler
Equations ⇒ Adg0 M2 Adg0 Adg0 V̇2 − adT(Adg V̇2 ) Adg0 M2 Adg0 Adg0 V1 = Ad−T
g 0 F2
0
Coordinate-

Since adAdg0 V = Adg0 adV Ad−1

invariant
algorithms for
robot
g0 , we have
dynamics

Lagrange’s
Equations with
M2 V̇2 − adV2 M2 V2 = F2
Constraints
 Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

32
Cn Cn+1
C2
C1

Chapter 4
Robot
Dynamics and
Control

Lagrangian C0
Equations
Ci : Frame fixed to link i, located along the ith axis
Inertial
Properties of Fi : Generalized force link i − 1 exerting on link i, expressed in Ci
Rigid Body τi : Joint torque of link i
Dynamics of gi−1,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
an Open-chain ′
Manipulator

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

⎧ ⎡ ⎤
i−1,i

⎪ ⎢ 0 ⎥
Equations

⎪
⎪ ⎢ ⎥∶
⎪
⎪ ⎢ ⎥
Coordinate-

⎪
⎪
⎪⎢ zi ⎥
invariant Revolute joint.
ξ = ⎨⎣ ⎦
algorithms for

⎪ ⎡ ⎤
robot

⎪
⎪ ⎢ ⎥
⎪ ⎢ ⎥∶
dynamics
z
⎪
⎪ ⎢ ⎥
i
⎪
⎪ ⎢ ⎥
Lagrange’s Prismatic joint.
⎩⎣ ⎦
Equations with
Constraints
0
 Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

33

−1
Chapter 4
Robot
⇒ gi−1,i ⋅ ġi−1,i = ξ̂i ⋅ θ̇ i
Dynamics and
Control
Mi : Moment of inertia in Ci
−mi r̂i
Mi = [ m ir̂ I − m 2 ] I:
Lagrangian mI mi : Mass of link i
Equations
i i i i r̂i i Angular moment of inertia
Inertial
Properties of gi = gi−1 gi−1,i
V̂i = gi−1 ⋅ ġi = gi−1,i
−1
V̂i−1 gi−1,i + ξ̂i θ̇ i
Rigid Body

Dynamics of

Vi = Adg −1 Vi−1 + ξi θ̇ i
an Open-chain
Manipulator
i−1,i
Newton-Euler
−1 −1 −1 ˙
V̂˙ i = ġi−1,i V̂i−1 gi−1,i + gi−1,i V̂i−1 ġi−1,i + gi−1,i V̂i−1 gi−1,i + ξ̂i θ̈ i
Equations

−1 −1 −1 −1
= −gi−1,i V̂i−1 gi−1,i + gi−1,i
Coordinate-
invariant
algorithms for
ġi−1,i gi−1,i V̂i−1 gi−1,i gi−1,i ġi−1,i
+ g −1 V̂˙ g + ξ̂ θ̈
robot
dynamics
i−1,i i−1 i−1,i i i
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

34
= − ξ̂i θ̇ i (Adg −1 Vi−1 )∧ + (Adg −1 Vi−1 )∧ ξ̂i θ̇ i + (Adg −1 V̇i−1 )∧ + ξ̂i θ̈ i
⇒ V̇i = ξi θ̈ i + Adg −1 V̇i−1 − ad ξi θ̇ i (Adg −1 Vi−1 )
i−1,i i−1,i i−1,i

Chapter 4 i−1,i i−1,i

Robot
Dynamics and ◻ Forward Recursion:
i = 0 ∶ V0 = 0, V̇0 = [ 0 ]
Control g
Lagrangian

⎧
Equations

⎪
⎪
⎪ gi−1,i = gi−1,i (0)eξ̂i θ i
⎪
⎪
Inertial

⎪
Properties of

⎨ Vi = Adgi−1,i −1 Vi−1 + ξ i θ̇ i
Rigid Body

⎪
⎪
⎪
⎪
Dynamics of

⎪
⎩ V̇i = ξi θ̈ i + Adgi−1,i
⎪ −1 V̇i−1 − ad
ξ i θ̇ i (Adgi−1,i
−1 Vi−1 )
an Open-chain
Manipulator

Newton-Euler
Equations

Coordinate- ◻ Backward Recursion:

invariant
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
algorithms for
robot
dynamics i,i+1
Lagrange’s
Equations with τi = ξiT ⋅ Fi
Constraints
 Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

35

−1 ⋅ V0 + ξ 1 θ̇ 1
Chapter 4
Robot V1 = Adg0,1
Fn = AdTg−1 ⋅ Fn+1 + Mn V̇n − adTVn ⋅ (Mn Vn )
Dynamics and
Control

Lagrangian n,n+1

⎡ θ̇ ⎤
Define:
⎢ 1 ⎥
Equations

V = [ ⋮ ] ∈ R6n×1 , θ̇ = ⎢ ⎥
V1 ξ1 0 0
⎢ ⋮ ⎥∈R ,ξ=[ 0 ⋱ 0 ] ∈ R6n×n
Inertial
n
⎢ θ̇ n ⎥
Properties of
Vn ⎣ ⎦
Rigid Body
0 0 ξn
⎡ Adg −1 ⎤
⎢ ⎥
Dynamics of
F1 τ1 ⎢ ⎥
an Open-chain

F = [ ⋮ ] ∈ R6n×1 , τ = [ ⋮ ] ∈ Rn , P0 = ⎢ 0 ⎥ ∈ R6n×6
0,1

⎢ ⋮ ⎥
Manipulator

Fn τn ⎢ 0 ⎥
⎣ ⎦
Newton-Euler
Equations

Coordinate-
invariant
Pt = [0⋯0 Adg −1 ] ∈ R
T 6×6n
n,n+1
algorithms for
robot
dynamics

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

4.5 Coordinate-invariant algorithms for robot dynamics

36
V1 = Adg0,1
−1 ⋅ V0 + ξ 1 θ̇ 1

V2 − Adg −1 V1 = ξ2 θ̇ 2
Chapter 4 1,2
Robot
Dynamics and ⋮
Control
Vn − Adg −1 Vn−1 = ξn θ̇ n
⎡ 0 ⎤
Lagrangian
n−1,n

⎢ ⎥ ⎡ V1 ⎤
Equations
I 0 ⋯
⎢ −Adg1,2 ⋮ ⎥⎢ V ⎥
⎢ ⎥⎢ 2 ⎥
Inertial I ⋱
⇒⎢ ⎥⎢ ⋮ ⎥
−1
Properties of

⎢ 0 ⋱ ⋱ 0
I ⎥ ⎢ ⎥
⎢ ⎥ ⎣ Vn ⎦
Rigid Body

⎣ ⎦ ´¹¹ ¹ ¹ ¹¸ ¹ ¹ ¹ ¹ ¶
Dynamics of 0 0 −Adg −1
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ V
n−1,n
an Open-chain
Manipulator
G−1
⎡ Adg −1 ⎤ ⎡ ξ1 ⎤⎡ θ̇ 1 ⎤
⎥⎢ ⎥
Newton-Euler

⎢ ⎥ ⎢ ⎢ ⎥
Equations

⎢ 0 0,1 ⎥ ⎢ ⎥ ⎢ ⎥
=⎢ ⎥V + ⎢ ⎥⎢
⎥⎢ ⋮ ⎥
ξ θ̇
⎢ ⋮ ⎥ 0 ⎢
Coordinate-

⎥
2 2
⎢ 0 ⎥ ⎢ ξn ⎥
invariant
⋱
⎣ ⎦ ⎣ ⎦⎢ θ̇ n ⎥
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ´¹¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹⎦
⎣
algorithms for

´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
robot

¶
dynamics

Lagrange’s P0 ξ
Equations with
θ̇
Constraints Thus V = GP0 V0 + Gξ θ̇
 Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

37

where
⎡ I ⎤
⎢ Ad −1 ⎥
Chapter 4
0 0 ⋯ 0
⎢ ⎥
Robot

⎢ ⎥
Dynamics and I 0 ⋯ 0
G=⎢ ⎥ ∈ R6n×6n
Control g1,2
⎢ Ad Adg −1 I ⋱ ⋮ ⎥
⎢ ⋮ 1,3 ⎥
Lagrangian g −1
⎢ ⎥
2,3
I
⎢ Adg −1 ⎥
Equations
⋮ ⋱ 0
Inertial
Properties of ⎣ 1,n
Adg −1
2,n
⋯ Adg −1
n−1,n
I ⎦
Rigid Body

Dynamics of
an Open-chain V̇1 = ξ1 θ̈ 1 + Adg −1 V̇0 − ad ξ1 θ̇ 1 (Adg −1 V0 )
0,1 0,1
Manipulator

Newton-Euler

ξ 2 θ̇ 2 (Adg1,2
−1 V1 )
Equations
−1 V̇1 = ξ 2 θ̈ 2 − ad
V̇2 − Adg1,2
Coordinate-
invariant

V̇n − Adg −1 V̇n−1 = ξn θ̈ n − ad ξn θ̇ n (Adg −1 Vn−1 )

algorithms for
robot
dynamics
n−1,n n−1,n
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

38

⎡ ⋯ 0 ⎤ ⎡ V̇ ⎤ ⎡ Adg −1 ⎤ ⎡ ξ1 ⎤⎡ ⎤
⎢ −Ad −1 ⋮ ⎥ ⎥⎢ ⎥
I
⎥⎢ ⎥ ⎢ 0,1 ⎥ ⎢
0
⎢ ⋱ ⎥⎢
θ̈ 1
⎥
⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎢ θ̈ 2 ⎥
I 1
⎢ ⎥⎢ ⎥=⎢ ⎥ V̇0 +⎢ ⎥
⎥⎢ ⎥
Chapter 4 g 1,2
⋱ ⋱ ⋱
V̇2 ξ2
⎢ ⋮ ⎥ ⎢ ⋮ ⎥ ⎢ ⎢ ⋮
0
⎢ ⎥ ⎥
I ⎥⎢ ⎥ ⎢ ⎥ ⎢ ξn ⎥
Robot 0 0
⎢ 0 −Adg −1 ⎦⎢ ⎥
⎣ ⎦ ⎣ V̇n ⎦ ⎣ ⎦ ⎣ ⎣ θ̈ n ⎦
Dynamics and 0 0
Control
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
n−1,n
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
Lagrangian P0 ξ
G−1
Equations

⎡ −ad ξ θ̇ ⋯ ⎤ ⎡ Ad −1 ⎤
⎢ 0 0 ⎥⎢ ⎥
Inertial
⎢ −ad ξ 2 θ̇ 2 ⋱ ⋮ ⎥⎢ ⎥
⎢ ⎥⎢
Properties of 1 1 g 0,1
+⎢ ⎥⎢ ⎥ V0
0
⋮ ⋱ ⋱ ⋮ ⎥
0
⎢ ⎥⎢
Rigid Body
⎢ ⋯ 0 −ad ξ n θ̇ n ⎥
0 ⎥
Dynamics of ⎣ 0 ⎦⎣ 0 ⎦
an Open-chain
Manipulator
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
ad ξ θ̇
Newton-Euler
Equations

⎡ −ad ξ θ̇ ⋯ ⎤⎡ ⋯ 0 ⎤
⎢ ⎥ ⎢ Ad −1 0 ⋮ ⎥ ⎡ V1 ⎤
0 0
⋱
0 0
⎢ ⎥⎢ ⎥⎢ V ⎥
Coordinate-
−ad ξ 2 θ̇ 2 ⋱ ⋮
+⎢ ⎥⎢ ⎥⎢ 2 ⎥
1 1
invariant
⎢ ⎥⎢ 0 ⎢ ⎥ ⎥
0 g 1,2
⎢ ⋮ ⋱ ⋱ ⎥⎢ ⋱ ⋱ ⎢ ⋮
0 ⎥ ⎥
algorithms for 0
⎢ ⎥⎢ ⎥ ⎣ Vn ⎦
0
⎣ ⋯ −ad ξ n θ̇ n ⎦⎣ ⎦
robot
dynamics 0 0 0 0 Adg −1
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
n−1,n

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

4.5 Coordinate-invariant algorithms for robot dynamics

39

Chapter 4 Thus
Robot
Dynamics and
Control V̇ = G ⋅ ξ θ̈ + G ⋅ P0 V̇0 + G ⋅ ad ξ θ̇ P0 V0 + G ⋅ ad ξ θ̇ ΓV
Lagrangian
Equations
Finally the backward recursion:
Inertial

Fn = AdTg−1 Fn+1 + Mn V̇n − adTVn ⋅ (Mn Vn )

Properties of
Rigid Body

Dynamics of n,n+1

AdTg−1 Fn + Mn−1 V̇n−1 − adTVn−1 ⋅ (Mn−1 Vn−1 )

an Open-chain
Manipulator Fn−1 =
n−1,n
Newton-Euler
Equations ⋮
F1 = AdTg−1 F2 + M1 V̇1 − adTV1 (M1 V1 )
Coordinate-
invariant
algorithms for 1,2
robot
dynamics

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

4.5 Coordinate-invariant algorithms for robot dynamics

40
⎡ ⎤
⎢ I −AdTg−1 0 ⎥ ⎡ ⎤
⎢ 0 ⎥ ⎡ F1 ⎤ ⎡ M 1 ⎤⎢ V̇1 ⎥
⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥
⎢ ⎥ ⎢ F2 ⎥ ⎢ ⎥⎢ ⎥
1,2
Chapter 4
⇒⎢⎢ 0 I ⋱ 0 ⎥⎢
⎥⎢
⎥=⎢
⎥ ⎢
M2 ⎥⎢
⎥⎢
V̇2 ⎥
⎥
⎢ ⋱ −AdTg−1 ⎥⎥⎢⎢ ⋮ ⎥⎥ ⎢⎢ ⋱ ⎥⎢ ⎥
Robot
⎢ ⎥⎢ ⎥
⋮ ⋱ ⋮
⎢ n−1,n ⎥⎣ Fn ⎦ ⎣ ⎦⎢ ⎥
Dynamics and
⎢ ⎥
M ⎣ ⎦
´¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
Control
⎣ 0 ⋯ 0 I ⎦
n V̇n
⎡ ⎤
Lagrangian M

⎢
0
⎥ ⎡ −adT ⎤
⎢ ⎥ M1
Equations

⎢ ⎥
⎥ Fn+1 + ⎢ ⎥[
V1
+⎢ ][ ⋮ ]
⋮ V1
⎢ T ⎥
Inertial

⎢ ⎥ ⎢ ⎥
Properties of 0 ⋱ ⋱
⎢ Adg −1 ⎥ ⎣ Vn ⎦
Mn Vn
⎣ n,n+1 ⎦
Rigid Body T −ad
Dynamics of
an Open-chain ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶ ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
T
Manipulator
PtT adV
Newton-Euler
Equations

F = GT M V̇ + GT PtT Fn+1 +GT ⋅ adTV MV

±
Coordinate-
invariant
algorithms for
robot Ft
dynamics

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

4.5 Coordinate-invariant algorithms for robot dynamics

41

τ = ξT ⋅ F
Chapter 4
Robot
τ = ξ T GT M V̇ + ξ T GT PtT Ft + ξ T GT ⋅ adTV MV
Dynamics and
Control = ξ T GT M(Gξ θ̈ + GP0 V̇0 + G ⋅ ad ξ θ̇ P0 V0 + G ⋅ ad ξ θ̇ ΓV)
Lagrangian ⇒
Equations + ξ T GT PtT Ft + ξ T GT ⋅ adTV MV
= ξ T GT MGξ θ̈ + ξ T GT MGP0 V̇0 + ξ T GT MG ⋅ ad ξ θ̇ ΓV
Inertial
Properties of
Rigid Body

Dynamics of + ξ T GT PtT Ft + ξ T GT ⋅ adTV MV

an Open-chain
Manipulator Finally we get:
³¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ · ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹µ ³¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ·¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ µ
Newton-Euler
M(θ) C(θ,θ̇)

ξ T GT MGξ θ̈ + ξ T GT (MG ⋅ ad ξ θ̇ Γ + adTV M)Gξ θ̇

Equations

Coordinate-
invariant
+ ξ T GT MGP0 V̇0 + ξ T GT PtT Ft = τ
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹¶ ´¹¹ ¹ ¹ ¹ ¹ ¸¹ ¹ ¹ ¹ ¹ ¹ ¶
algorithms for
robot

JtT (θ)
dynamics
ϕ(θ)
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.5 Coordinate-invariant algorithms for robot dynamics

42

M(θ)θ̈ + C(θ, θ̇) + ϕ(θ) + JtT (θ)Ft = τ

Chapter 4
Robot
Dynamics and
Control

Lagrangian
Equations
M(θ) = ξ T GT MGξ
Inertial C(θ, θ̇) = ξ T GT (MGad ξ θ̇ Γ + adTV M)Gξ θ̇
Properties of

ϕ(θ) = ξ T GT MGP0 V̇0

Rigid Body

Dynamics of
an Open-chain
Manipulator
Jt = Pt Gξ
Newton-Euler Property 3:
Equations
Γn = 0,
G = (I − Γ)−1 = I + Γ + Γ2 + ⋯ + Γn−1
Coordinate-
invariant
algorithms for
robot
dynamics I + ΓG = G
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

43
Definition: Holonomic constraints
Chapter 4
Given generalized coordinates q = (q1 , . . . , qn ) ∈ E, a holonomic
Robot constraint is a set of constraint equations:
hi (q) = 0, i = 1, . . . , k
Dynamics and
Control

Lagrangian
Equations
Q = h (0) is a manifold of dim n − k ≜ m if the constraints are
−1

linearly independent.
Tq Q ∶ {V ∈ Tq E∣dhi ⋅ V = 0, ∀i = 1, . . . , k} ⊂ Tq E
Inertial
Properties of
Rigid Body
subspace of permissible velocities.
Tq∗ Q⊥ ∶ {f ∈ Tq∗ E∣⟨f , v⟩ = 0, ∀V ∈ Tq Q} = span{dh1 , . . . , dhk }
Dynamics of
an Open-chain
Manipulator

Newton-Euler
subspace of constraint forces.
Equations

Coordinate- Definition: Constraint forces

invariant
∂h T
algorithms for Γ= ∂q ⋅λ
robot
k
dynamics
λ ∈ R is the vector of relative magnitudes of constraint forces.
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

44

Definition: Pfaffian Constraints

Chapter 4
Robot A Pfaffian constraint has the form:
Dynamics and
Control
A(q)q̇ = 0, A(q) ∈ Rk×n
Given a Pfaffian constraint,
∆q = {Vq ∈ Tq E∣A(q) ⋅ Vq = 0} ⊂ Tq E
Lagrangian
Equations

Inertial
Properties of Distribution of permissible velocities.
Rigid Body

Dynamics of
an Open-chain
Manipulator Definition:
Newton-Euler A(q)q̇ = 0 is holonomic (or integrable) iff ∆q is an involutive
Equations
distribution, or iff
∃hi ∶ E ↦ R, i = 1, . . . , k s.t. ∆q = Tq Q, Q = h−1 (0)
Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
 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
Inertial
Properties of
T(q, q̇) = q̇T ⋅ M(q) ⋅ q̇
Rigid Body
2
Dynamics of
an Open-chain § Potential energy:
Manipulator

Newton-Euler V(q)
Equations

Coordinate-
invariant § Lagrangian:
algorithms for
robot
dynamics L(q, q̇) = T(q, q̇) − V(q)
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

46

Chapter 4
Robot
◻ Lagrange’s equations with constraints:
M(q)q̈ + C(q, q̇) + N(q) + AT (q)λ = F
Dynamics and
Control

Lagrangian
Equations

Inertial
Properties of
◻ Explicit solution for constraint forces:
Rigid Body
A(q)q̈ + Ȧ(q)q̇ = 0
(AM −1 AT )λ = AM −1 (F − C − N) + Ȧq̇
Dynamics of
an Open-chain
Manipulator

Newton-Euler
Equations
λ = (AM −1 AT )−1 (AM −1 (F − C − N) + Ȧq̇)
Coordinate-
invariant
algorithms for
robot
dynamics

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

4.6 Lagrange’s Equations with Constraints

47

◇ Example:
x2 + y2 = l 2
Chapter 4
Robot

[x y] [ ẋẏ ] = 0
Dynamics and

²
Control

Lagrangian
y
Equations A(q)

L(q, q̇) = m(ẋ2 + ẏ2 ) − mgy

Inertial
x 1
Properties of
Rigid Body l 2
[ m0 m ] [ ÿ ] + [ mg ] + [ y ] λ = 0
0 ẍ 0 x
(x, y)
Dynamics of
θ
λ = (AM −1 AT )−1 (AM −1 (F − C − N) + Ȧq̇)
an Open-chain
Manipulator

= − 2 (gy + ẋ2 + ẏ2 )

Newton-Euler
Equations m
Coordinate- mg l
= ∥[ y ] λ∥ = y + (ẋ2 + ẏ2 )
invariant x mg m
algorithms for
robot l l
dynamics

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

4.6 Lagrange’s Equations with Constraints

48
◻ Lagrange-d’Alembert formulation:
Given the Pfaffian constraint A(q)q̇ = 0 and virtual displacement
δq ∈ Rk , we have:
Chapter 4
Robot
Dynamics and
Control
Theorem 1 (D’alembert Principle):
Lagrangian
Equations Forces of constraints do no virtual work!
Inertial (AT (q)λ) ⋅ δq = 0 for A(q)δq = 0
Properties of
Rigid Body

Dynamics of
an Open-chain
Manipulator
1717–1783
Newton-Euler
Equations
Theorem 2 (Lagrange-d’Alembert Equation):
(
Coordinate-
invariant d ∂L ∂L
algorithms for − − τ) ⋅ δq = 0, A(q)δq = 0
robot dt ∂q̇ ∂q
dynamics

Lagrange’s
Equations with
Constraints
1736-1813
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

49

Chapter 4 Let A(q) = [A1 (q) A2 (q)], and A2 (q) ∈ Rk×k is invertible, then
δq1 ∈ Rn−k are free variables:
Robot
Dynamics and
Control

Lagrangian
Equations
δq2 = −A−1
2 (q)A1 (q)δq1

⇒(
Inertial d ∂L ∂L
Properties of − − τ) ⋅ δq
Rigid Body dt ∂q̇ ∂q
Dynamics of

=( − τ1 ) ⋅ δq1 + ( − τ2 ) ⋅ δq2
an Open-chain d ∂L ∂L d ∂L ∂L
Manipulator − −
Newton-Euler
dt ∂q̇1 ∂q1 dt ∂q̇2 ∂q2

=( − τ1 ) ⋅ δq1 + ( − τ2 ) ⋅ (−A−1
2 A1 )δq1
Equations
d ∂L ∂L d ∂L ∂L
Coordinate- − −
invariant dt ∂q̇1 ∂q1 dt ∂q̇2 ∂q2
algorithms for
robot
dynamics

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

4.6 Lagrange’s Equations with Constraints

50

Chapter 4
Robot As δq1 ∈ Rn−k is free,
Dynamics and
Control

( − τ1 ) − AT1 A−T
2 ( − τ2 ) = 0
Lagrangian d ∂L ∂L d ∂L ∂L
Equations − −
Inertial
dt ∂q̇1 ∂q1 dt ∂q̇2 ∂q2
Properties of
Rigid Body

Dynamics of Lagrange-d’Alembert equation

an Open-chain
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

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

4.6 Lagrange’s Equations with Constraints

51

◇ Example: Dynamics of a rolling disk

Chapter 4

⎧
Robot
Pfaffian constraint:
⎪
⎪ ẋ − ρ cos θ̇ ϕ̇ = 0
Dynamics and

⎨
Control ϕ

⎪
⎪
Lagrangian

⎩ ẏ − ρ sin θ ϕ̇ = 0
θ
Equations (x, y)
Inertial
Properties of

⇒ A(q)q̇ = [ 0 1 0 −ρ sin θ ] q̇ = 0
Rigid Body 1 0 0 −ρ cos θ
Dynamics of

L(q, q̇) = 21 m(ẋ2 + ẏ2 ) + 21 I1 θ̈ 2 + 21 I2 ϕ̇2

an Open-chain
Manipulator

Lagrange-d’Alembert equation:
⎡ m ⎤ ⎡ 0 ⎤
Newton-Euler

⎛⎢ ⎥ ⎢ 0 ⎥⎞
Equations

⎢ m ⎥ q̈ − ⎢ ⎥
⎢ ⎥ ⎢ τθ ⎥⎠ ⋅ δq = 0
Coordinate-

⎝⎢ ⎥ ⎢ ⎥
I1
⎣ 2 ⎦ ⎣ ϕ ⎦
invariant
algorithms for
robot
I τ
dynamics where A(q) ⋅ δq = 0.
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

52
As
δx = ρ cos θ ⋅ δϕ
Chapter 4 {
Robot δy = ρ sin θ ⋅ δϕ
Dynamics and
Control the equation of motion is:
Lagrangian
([ mρc0
θ mρsθ
0 ] [ ẍ ] + [ I1 0 ] [ θ̈ ] − [ τθ ]) ⋅ [ δθ ] = 0
ÿ 0 I2 τϕ δϕ
Equations
ϕ̈
⇒ [ mρc 0 ] [ ẍ ] + [ I1 0 ] [ θ̈ ] = [ τθ ]
Inertial
Properties of 0
Rigid Body θ mρsθ ÿ 0 I2 ϕ̈ τϕ

⎧
As
⎪
Dynamics of

⎪ ẍ = ρ cos θ ⋅ ϕ̈ − ρ sin θ ⋅ θ̇ ϕ̇
an Open-chain

⎨
Manipulator

⎪ ÿ = ρ sin θ ⋅ ϕ̈ + ρ cos θ ⋅ θ̇ ϕ̇
⎪
⎩
Newton-Euler
Equations

⇒ [ 01 I + mρ2 ] [ θ̈ ] = [ τϕθ ]
Coordinate- I 0 τ
invariant
2 ϕ̈
Solve for (θ(t), ϕ(t)), and then solve for (x(t), y(t)) from:
algorithms for

⎧
robot

⎪
⎪ ẋ = ρ cos θ ⋅ ϕ̇
dynamics

⎨
Lagrange’s

⎪
⇐ 1st order differential equation
⎪
Equations with

⎩ ẏ = ρ sin θ ⋅ ϕ̇
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

53

◻ Nature of nonholonomic constraints:

Consider q = (r, s) ∈ R2 × R with Pfaffian constraint,
Chapter 4

ṡ + aT (r)ṙ = 0, a(r) ∈ R2
Robot
Dynamics and
Control

Lagrangian Lagrangian
Equations
L = L(r, ṙ, ṡ)
Inertial
Properties of and constrained Lagrangian
Lc (r, ṙ) = L(r, ṙ, −aT (r)ṙ)
Rigid Body

Dynamics of
an Open-chain ⇒ Lagrange’s equation:
Manipulator
d ∂Lc ∂Lc
Newton-Euler − = 0, i = 1, 2
Equations dt ∂ṙi ∂ri
⎛ ∂L ∂L ∂aj ⎞
⇒ ( − ai (r) ) −
d ∂L ∂L
∑
Coordinate-
ṙj = 0
⎝ ∂ri ∂ṡ j ∂ri ⎠
invariant −
algorithms for
robot
dt ∂ṙi ∂ṡ
dynamics

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

4.6 Lagrange’s Equations with Constraints

54

∂L ⎛ ∂aj ⎞
⇒( ) − ai (r) ( − )= ȧi (r) − ∑ ṙj (∗)
d ∂L ∂L d ∂L ∂L
∂ṡ ⎝ j ∂ri ⎠
Chapter 4 −
Robot dt ∂ṙi ∂ri dt ∂ṡ ∂s
´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
Dynamics and
Control

Lagrangian
≠0
Equations If the constraint is holonomic, i.e.
Inertial

ai (r) =
Properties of ∂h
Rigid Body for some h ∶ E ↦ R
Dynamics of ∂ri
an Open-chain
Manipulator
then RHS (right hand side) of (∗) equals
Newton-Euler

∂L ⎛ ∂2 h ⎞
Equations
∂2 h
∑ ṙj − ∑
Coordinate-

∂ṡ ⎝ j ∂ri ∂rj j ∂rj ∂ri ⎠

invariant ṙj = 0
algorithms for
robot
dynamics

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

4.6 Lagrange’s Equations with Constraints

55
◻ Metric, duality and orthogonality on Tq E:
xn
Tq Q
Chapter 4
Robot ⋯
Dynamics and
x1 q 1
Control K = q̇T M(q)q̇
Lagrangian
2
Q = h−1 (0)
Equations 1
= ≪ q̇, q̇ ≫M
Inertial
Properties of
2
Rigid Body

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

Dynamics of
an Open-chain

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

Manipulator

Newton-Euler

Tq E = Tq Q ⊕ Tq Q⊥ , Tq∗ E = Tq∗ Q ⊕ Tq∗ Q⊥

Equations

Coordinate-
invariant
algorithms for Definition:
M b ∶ Tq E ↦ Tq∗ E, ⟨M ♭ V1 , V2 ⟩ = V1T MV2 =≪ V1 , V2 ≫M
robot
dynamics

−1
M ♯ ∶ Tq∗ E ↦ Tq E, M ♯ = M ♭
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

56

Chapter 4
Robot
Dynamics and Reciprocal
Control

Lagrangian
Equations

Inertial
Properties of
Rigid Body
Property 4:
and dqi , i = 1, . . . , n of Tq E and Tq∗ E
Dynamics of
∂
an Open-chain Under the basis ∂qi
respectively, the matrix representation of M ♭ and M ♯ is M and
Manipulator

Newton-Euler

M −1 respectively.
Equations

Coordinate-
invariant
algorithms for Property 5:
M ♯ (Tq∗ Q) = Tq Q
robot
dynamics

Lagrange’s
Equations with M ♯ (Tq∗ Q⊥ ) = Tq Q⊥
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

57

Given
Chapter 4 h ∶E ↦ Rk , m = n − k
Robot

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

Lagrangian
Equations h∗ ≜ Tq∗ h ∶Th(q)
∗
Rk ↦ Tq∗ E
Inertial
Properties of we have
Rigid Body Property 6:
Dynamics of ker h∗ = Tq Q, h∗ (Tq Q⊥ ) = Th(q) Rk , h∗ (Th(q)
∗
Rk ) = Tq∗ Q⊥
h∗
an Open-chain
Manipulator

Newton-Euler Tq∗ E Tq∗ Rk

Equations

M2♯ M2♯ = h∗ ○ M ♯ ○ h∗
Coordinate-
invariant
algorithms for M♯
robot
dynamics

Lagrange’s Tq E Th(q) Rk
Equations with
Constraints
h∗
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

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

(I − Pω )f2 = f2 , ∀f2 ∈ Tq∗ Q⊥

Lagrangian
Equations

Inertial
Properties of Proof :
Given f1 ∈ Tq∗ Q, M ♯ (f1 ) ∈ Tq Q = ker h∗ , then (I − Pω )(f1 ) = 0. For
Rigid Body

f2 ∈ Tq∗ Q⊥ , ∃λ ∈ Rn−m s.t. f2 = h∗ λ, and

Dynamics of
an Open-chain

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

Newton-Euler

= h∗ λ = f2
Equations

⇒ Pω ∶ Tq∗ E ↦ Tq∗ Q
Coordinate-
invariant
algorithms for
robot is a well-defined projection map. Similarly,
PT ∶ Tq E ↦ Tq Q, PT = I − M ♯ h∗ M2b h∗
dynamics

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

4.6 Lagrange’s Equations with Constraints

59

and (I − PT ) ∶ Tq E ↦ Tq Q⊥ are projection maps.

Chapter 4
Robot Lemma 2:
Dynamics and Pω M = MPT
Pω h∗ = h∗ PT = 0
Control

Lagrangian
Equations

Inertial PT = PTω
Properties of
Rigid Body

Dynamics of
an Open-chain
For nonholonomic constraints:
Manipulator h∗ ← A(q)
Newton-Euler
Equations h∗ ← A∗ (q)
Coordinate- Tq Q ← ∆ q
Tq∗ Q⊥ ← span{ai (q), i = 1, . . . , k}
invariant
algorithms for
robot
dynamics
application in hybrid velocity/force control.
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

60
◻ Lagrange’s equations of motion:
Chapter 4 M(q)q̈ + C(q, q̇) + N + AT (q)λ = τ
λ = (AM −1 AT )−1 (AM −1 (τ − C − N) + Ȧq̇)
Robot
Dynamics and
Control

Lagrangian
Equations
M(q)q̈ + AT (AM −1 AT )Ȧq̇ + Pω C + Pω N = Pω τ
Inertial Pω = I − AT (AM −1 AT )−1 AM −1
Properties of
Rigid Body

Dynamics of C̃ = Pω C
an Open-chain
Manipulator
Ñ = Pω N
Newton-Euler
Equations τ̃ = Pω τ
M θ̈ + AT (AM −1 AT )−1 Ȧθ̇ = Pω M θ̈ ≜ M̃ θ̈ ∶ intertia forces in Tq∗ Q
Coordinate-
invariant

Definition: Dynamics in Tq∗ Q

algorithms for
robot
dynamics

Lagrange’s M̃ θ̈ + C̃ + Ñ = τ̃
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

61
(I − Pω )(Mq̈ + C + N) = (I − Pω )τ + AT τ
Let
Chapter 4
Robot
PT = I − M −1 AT (AM −1 AT )−1 A
Dynamics and
Control
and
Pω M = MPT
∗ ⊥
Lagrangian
Equations
then the dynamics in T Q :
Inertial

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

Properties of
Rigid Body

Dynamics of
an Open-chain ◻ Geometric Interpretation:
∇↔M
Manipulator

Mq̈ + C + N = τ + AT λ ⇔ M∇q̇ q̇ = τ − N + AT λ
Newton-Euler
Equations

Coordinate-
invariant ∇˜ ↔ induced metric on Tq Q
S ∶TQ ⊗ TQ ÐÐÐÐÐÐ→ N(Q) :2nd fundamental form
algorithms for
robot
dynamics

Lagrange’s ↑ ↑
Equations with
Constraints tangent vector field normal vector field
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

62

Chapter 4
Robot
∇X Y = ∇ ˜ X Y + S(X, Y)
−1
M (I − PT )(q̈ + M C) = (I − Pω )(τ − N) + AT λ
Dynamics and
Control

Lagrangian ´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
Equations S(q̇,q̇)
Inertial
Properties of
MS(q̇, q̇): centrifugal force due to curvature of Q in E
Rigid Body

⎫
⎪
Dynamics of

⎪
⎪
an Open-chain
˜ q̇ q̇ = τ̃ − Ñ
M∇
⎬ for hybrid control design
Manipulator

T ⎪
MS(q̇, q̇) = (I − Pω )(τ − N) + A λ ⎪
⎪
Newton-Euler

⎭
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

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

4.6 Lagrange’s Equations with Constraints

63
◇ Example: Dynamics of a Spherical Pendulum

K = m(ẋ2 + ẏ2 + ż2 )

Chapter 4 1
Robot
Dynamics and
Control
2
1
Lagrangian = q̇T Mq̇
Equations 2
Inertial
Properties of
q = (x, y, z)T , M = mI
Rigid Body

Dynamics of
h ∶qT q − r2 = 0
an Open-chain

A = (x, y, z), M2♯ = AM ♯ AT = r2 /m

Manipulator

⎡ y2 + z 2 ⎤
Newton-Euler

1 ⎢ ⎥
Equations

Pω = 2 ⎢ ⎥
−xy −xz
⎢ x2 + z 2 ⎥
Coordinate-

r ⎢ −zx ⎥
−yz −yz
⎣ ⎦
invariant
algorithms for −zy x2 + y2
⎡ x2 xy xz ⎤
robot

1 ⎢ ⎥
dynamics

I − Pω = 2 ⎢ yx y2 yz ⎥
r ⎢ zx zy z2 ⎥
⎢
Lagrange’s

⎥
⎣ ⎦
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

64

PT = PTω = Pω
Chapter 4
(µ, ν): Spherical coordinates
q = (r cos µ cos ν, r cos µ sin ν, r sin µ)T
Robot
Dynamics and

˜ q̇ q̇ = PT (∇q̇ q̇)
Control

Lagrangian ∇
Equations
−r sin µ cos ν −r sin ν
Inertial
Properties of = [ −r sin µ sin ν r cos ν ] [ vv21 ]
Rigid Body r cos µ 0
Dynamics of
an Open-chain
S(q̇, q̇) = (I − PT )(∇q̇ q̇)
Manipulator r cos µ cos ν
Newton-Euler = (− µ̇2 − cos2 µ ν̇ 2 ) [ r cos µ sin ν ]
Equations r sin µ
Coordinate- where
invariant
algorithms for v1 = µ̈ + sin µ cos µ ν̇ 2
robot
dynamics v2 = cos µ ν̈ − 2 sin µ µ̇ν̇
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

65

Chapter 4
◻ Control Algorithm:
Robot
Dynamics and
Control
1 holonomic constraints:
Lagrangian
q̃ ∶ coordinates of Q
Equations
q = ψ(q̃) ⇒ q̇ = J ⋅ q̃˙
Inertial

τ = MJ(q̃¨ d − Kv ˙ẽ − Kp ẽ) + C1 + N + AT (−λd + KI ∫ (λ − λ ))

Properties of
Rigid Body d
Dynamics of
an Open-chain 2 nonholonomic constraints:
Let J(q) ∈ Rn×m be s.t. AJ = 0. Write q̇ = J ⋅ u for some u
Manipulator

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

Newton-Euler
Equations
d
Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
 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
Contact constraint:

vz = 0 ⇔ [0 0 1 0 0 0]Adg −1 Vof = 0
Lagrangian
Equations
fl f

⇒ Holonomic constraint:
Inertial
Properties of

η = (αoT , αfT , ψ): Parametrization of Q

Rigid Body

Dynamics of
an Open-chain Pω = diag(1, 1, 0, 1, 1, 1)
Manipulator

Newton-Euler Newton-Euler Equations of motion:

M V̇of − adTVof MVof = Fm + G + AT λ
Equations

⎡ Rψ Mo 0 ⎤
Coordinate-

⎢ −Mf ⎥⎡ ⎤
⎢ 0 ⎥ ⎢ α̇o ⎥
invariant

Vof = ⎢ Rψ Ro Ko Mo −Ro Kf Mf 0 ⎥ ⎢ α̇f ⎥ ≜ J η̇

algorithms for
0 0
⎢ ⎥ ⎢ ψ̇ ⎥
robot

⎢ −To Mo 1 ⎥ ⎣ ⎦
dynamics

Lagrange’s ⎣ −Tf Mf ⎦
MJ η̈ + C1 = Fm + G + AT λ
Equations with
Constraints ∗
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

67
Pω (∗) ∶
Chapter 4
M̃ η̈ + C̃1 = B1 Fm + B1 G
Robot
Dynamics and −ϕ3 − λ = b2 Fm + b2 G
F̂ = [f1 f2 f4 f5 f6 ]T = M̃(η̈d − Kv ˙ẽ − Kp ẽ) + C̃1 − B1 G
Control

Lagrangian

∫ (λ − λ ) − b G
Equations

Inertial f3 = −ϕ3 − λd + KI d 2
Properties of

τ = JsT Fm
Rigid Body

Dynamics of
an Open-chain
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

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

4.6 Lagrange’s Equations with Constraints

68
◇ Example: 6-DoF manipulator rolling on a sphere
⎡ 1 0 0 0 0 0 ⎤
⎢ 0 1 0 0 0 0 ⎥
⎢ ⎥
⎢ 0 0 1 0 0 0 ⎥ Vlo lf = 0
Chapter 4

⎢ 0 0 0 0 0 1 ⎥
⎣ ⎦
Robot
Dynamics and

´¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¸¹¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¹ ¶
Control

Lagrangian
Equations
Ȧ1 (q)
T
Inertial fc = A1 λ, λ ∈ R4
Properties of
ωx
Rigid Body
[ ωy ] = −R0 (Kf + Rψ Ko Rψ )Mf α̇f
Dynamics of
an Open-chain
Manipulator Vof = Adgflf ⋅ Vlo lf
⎡ ⎤
⎢ 0 ⎥
Newton-Euler

⎢ ⎥
⎢ ⎥ α̇f
Equations
0
⎢ 0 ⎥
⎢ −Ro (Kf + Rψ Ko Rψ )Mf ⎥
Coordinate- = Adgflf
⎢ ⎥
invariant

⎣ ⎦
algorithms for
robot o
≜ Jf α̇f
dynamics

span{Jf }: Not involutive

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

4.6 Lagrange’s Equations with Constraints

69

MJf α̈f + (M J̇f α̇f − adTJf α̇f MJf α̇f ) = Fm + G + AT λ

Chapter 4
Robot Fm = MJf (α̈fd − Kp (α̇f − α̇fd )) + (M J̇f α̇f − adTJf α̇f MJf α̇f )
Dynamics and

+ AT (−λd + ∫ (λ − λ )) − G
Control

Lagrangian
d
Equations

Inertial
Properties of ◇ Example: Redundant parallel manipulator
θ = (θ 1 , . . . , θ 6 ) ∈ E
Rigid Body

Dynamics of

θ a = (θ 1 , θ 3 , θ 5 )
an Open-chain
Manipulator

Newton-Euler
θ p = (θ 2 , θ 4 , θ 6 )
⎡ ⎤
Equations

⎢ xa + lc1 + lc12 − xb − lc3 − lc34 ⎥

⎢ ⎥
Coordinate-

H(θ) = ⎢ ⎥=0
ya + ls1 + ls12 − yb − ls3 − ls34
⎢ ⎥
invariant

⎢ ⎥
algorithms for
xa + lc1 + lc12 − xc − lc5 − lc56
⎣ ⎦
robot
dynamics ya + ls1 + ls12 − yc − ls5 − ls56
Lagrange’s
Equations with
Constraints
 Chapter 4 Robot Dynamics and Control

4.6 Lagrange’s Equations with Constraints

70
Mi (θ) ∈ R2×2 : ith chain
Chapter 4 M(θ) = diag(M1 (θ), . . . , M3 (θ))
Robot
Dynamics and M(θ)θ̈ + C + N = τ + AT λ
Control
If all joints are actuated,
Lagrangian
Equations
Position control of end-effector
+
Inertial internal grasping force
Properties of
As τ2 , τ4 , τ6 = 0,
θ̃ ∈ R2 ∶ local parametrization of Q = H −1 (0)
Rigid Body

Dynamics of
an Open-chain
Manipulator θ = ψ(θ̃) ∶embedding of Q in E
Newton-Euler
Equations
θ̇ = J θ̃˙
Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
 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

Pω MJ θ̃¨ + Pω (C1 + N) = Pω τ
Robot
Dynamics and

τ̃ = (τ1 , τ3 , τ5 )
Control

Lagrangian

P̃ω = (P1 , P3 , P5 )
Equations

Inertial
Properties of
Rigid Body τ̂ = P̂ω τ̃ = Pω τ ∈ R6
τ̂ = Pω MJ(θ̃¨d − Kv ˙ẽ − Kp ẽ) + Pω (C1 + N)
Dynamics of
an Open-chain
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints