2nd International Summer School on Geometric Methods in Robotics, Mechanism Design and Manufacturing Research-Lecture 04 Robot Dynamics and Control

1. Chapter 4 Manipulator Dynamics
1
Chapter Lecture Notes for
Manipulator
Dynamics
A Geometrical Introduction to
Introduction
Lagrange’s
Robotics and Manipulation
Equations
Dynamics of
Open-chain
Richard Murray and Zexiang Li and Shankar S. Sastry
Manipulators CRC Press
Coordinate
Invariant
Algorithms
Lagrange’s Zexiang Li and Yuanqing Wu
Equations with
Constraints
ECE, Hong Kong University of Science & Technology
June ,

2. Chapter 4 Manipulator Dynamics
2
Chapter Manipulator Dynamics
Chapter
Manipulator
Dynamics
Introduction
Introduction
Lagrange’s
Equations Lagrange’s Equations
Dynamics of
Open-chain
Manipulators
Dynamics of Open-chain Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Coordinate Invariant Algorithms
Equations with
Constraints
Lagrange’s Equations with Constraints

3. Chapter 4 Manipulator Dynamics
4.1 Introduction
3
Definition: Dynamics
Physical laws governing the motions of bodies and aggregates of
Chapter
Manipulator bodies.
Dynamics
Introduction
Lagrange’s
Equations
Dynamics of
Open-chain
Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

4. Chapter 4 Manipulator Dynamics
4.1 Introduction
3
Definition: Dynamics
Physical laws governing the motions of bodies and aggregates of
Chapter
Manipulator bodies.
Dynamics
Introduction ◻ A short history:
Lagrange’s
Equations
“Everything happens for a reason.”
Dynamics of
Open-chain Aristotle (384 BC - 322 BC)
Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

5. Chapter 4 Manipulator Dynamics
4.1 Introduction
3
Definition: Dynamics
Physical laws governing the motions of bodies and aggregates of
Chapter
Manipulator bodies.
Dynamics
Introduction ◻ A short history:
Lagrange’s
Equations
“Everything happens for a reason.”
Dynamics of
Open-chain Aristotle (384 BC - 322 BC)
Manipulators
Coordinate
Invariant
Algorithms Experiments with cannon balls from the tower of Pisa.
Lagrange’s G. Galilei (1564 - 1642)
Equations with
Constraints

6. Chapter 4 Manipulator Dynamics
4.1 Introduction
3
Definition: Dynamics
Physical laws governing the motions of bodies and aggregates of
Chapter
Manipulator bodies.
Dynamics
Introduction ◻ A short history:
Lagrange’s
Equations
“Everything happens for a reason.”
Dynamics of
Open-chain Aristotle (384 BC - 322 BC)
Manipulators
Coordinate
Invariant
Algorithms Experiments with cannon balls from the tower of Pisa.
Lagrange’s G. Galilei (1564 - 1642)
Equations with
Constraints
Laws of motion.
I. Newton (1642 - 1726)
(Continues next slide)

7. Chapter 4 Manipulator Dynamics
4.1 Introduction
4
Chapter
Manipulator
Laws of motion from particles to rigid bodies.
Dynamics
L. Euler (1707 - 1783)
Introduction
Lagrange’s
Equations
Dynamics of
Open-chain
Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

8. Chapter 4 Manipulator Dynamics
4.1 Introduction
4
Chapter
Manipulator
Laws of motion from particles to rigid bodies.
Dynamics
L. Euler (1707 - 1783)
Introduction
Lagrange’s
Equations
Calculus of Variation and the Principles of least action.
Dynamics of
Open-chain J. Lagrange (1736 - 1813)
Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

9. Chapter 4 Manipulator Dynamics
4.1 Introduction
4
Chapter
Manipulator
Laws of motion from particles to rigid bodies.
Dynamics
L. Euler (1707 - 1783)
Introduction
Lagrange’s
Equations
Calculus of Variation and the Principles of least action.
Dynamics of
Open-chain J. Lagrange (1736 - 1813)
Manipulators
Coordinate
Invariant
Algorithms Quaternions and Hamilton’s Principle.
Lagrange’s W. Hamilton (1805 - 1865)
Equations with
Constraints
† End of Section †

10. Chapter 4 Manipulator Dynamics
4.2 Lagrange Equations
5
◻ A simple example:
Newton’s Equation: Lagrangian Equation:
m¨ = Fx
Chapter
Manipulator x
m¨ = Fy − mg
Dynamics
Introduction
y
Momentum: Px = m˙
Lagrange’s
Equations x
Dynamics of Py = m˙y
= Fx , dt Py = Fy − mg
Open-chain
d d
Manipulators P
dt x
Coordinate
Invariant y
Algorithms
Lagrange’s Fy F
Equations with
Constraints m Fx
mg
x
Figure 4.1

11. Chapter 4 Manipulator Dynamics
4.2 Lagrange Equations
5
◻ A simple example:
Newton’s Equation: Lagrangian Equation:
m¨ = Fx
Chapter
− = Fx
Manipulator x d ∂L ∂L
m¨ = Fy − mg
Dynamics
y dt ∂˙ ∂x
x
Introduction
− = Fy
d ∂L ∂L
Momentum: Px = m˙
Lagrange’s
Equations x dt ∂˙ ∂y
y
Dynamics of Py = m˙y
Lagrangian function:
= Fx , dt Py = Fy − mg
Open-chain
d d
Manipulators P
dt x
Coordinate
Invariant y
⇔ L = T − V, Px =
∂L
, Py =
∂L
Algorithms ∂˙
x ∂˙
y
Lagrange’s Fy F Kinetic energy:
Equations with
m Fx
T = m(˙ + y )
Constraints
x ˙
mg
x Potential energy:
Figure 4.1
V = mgy

12. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
6
◻ Generalization to multibody systems:
qi , i = , . . . , n: generalized coordinates
Chapter Kinetic energy:
Manipulator
Dynamics y
T = T(q, q)
˙
Introduction m
q
Lagrange’s
Equations
Potential energy:
V = V(q)
Dynamics of
Open-chain m
Manipulators
q Lagrangian:
Coordinate
Invariant
L(q, q) = T(q, q) − V(q)
Algorithms m ˙ ˙
Lagrange’s
q
τ i , i = , . . . , n: external force on qi
Equations with
Constraints
x Lagrangian Equation:
Figure 4.2
d ∂L ∂L
− = τi , i = , . . . , n
dt ∂˙ i ∂qi
q

13. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
7
Example: Pendulum equation
Generalized coordinate:
θ∈S
Chapter y
Manipulator
Dynamics
Kinematics:
x = l sin θ, y = −l cos θ
Introduction x
Lagrange’s l
x = l cos θ ⋅ θ, y = l sin θ ⋅ θ
Equations
Dynamics of ˙ ˙ ˙ ˙ θ
Open-chain
Manipulators
Kinetic energy:
Coordinate T(θ, θ) = m(˙ + y ) = ml θ
˙ x ˙ ˙
Invariant mg
Algorithms Potential energy:
Figure 4.3
Lagrange’s
Equations with
V = mgl( − cos θ)
Constraints Lagrangian function:
L = T − V = ml θ − mgl( − cos θ), ⇒
˙ ∂L
˙
∂θ
= ml θ,
˙ ∂L
∂θ
= −mgl sin θ
Equation of motion:
d ∂L
˙
dt ∂ θ
− ∂L
∂θ
= τ ⇒ ml θ + mgl sin θ = τ
¨

14. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
8
Example: A spherical pendulum
Generalized coordinate:
⎡ l sin θ cos ϕ ⎤
Chapter
Manipulator
⎢ ⎥
Dynamics r(θ, ϕ) = ⎢ l sin θ sin ϕ
⎢ −l cos θ
⎥
⎥
⎣ ⎦
Introduction
Lagrange’s Kinetic energy:
Equations
Dynamics of
Open-chain T= m ˙
r = ml (θ + ( − cos θ)ϕ )
˙ ˙
Manipulators
θ
Coordinate
Invariant
Potential energy:
Algorithms
Lagrange’s V = −mgl cos θ
Equations with
Constraints
Lagrangian function:
ϕ mg
L(q, q) =
˙ ml (θ + ( − cos θ)ϕ ) + mgl cos θ
˙ ˙
Figure 4.4
(continues next slide)

15. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
9
⎧d
⎪ ∂L d
⎪
⎪
⎪ dt = (ml θ) = ml θ,
˙ ¨
⎪ ˙
⎨ ∂θ dt
⎪
Chapter
⎪
⎪ ∂L
Manipulator
⎪
⎪ = ml sin θ cos θ ϕ − mgl sin θ
˙
Dynamics ⎩ ∂θ
⎧
⎪d ∂L d
⎪
Introduction
⎪
⎪ dt = (ml sin θ ϕ) = ml sin θ ϕ + ml sin θ cos θ θ ϕ,
˙ ¨ ˙˙
⎪
⎪ ˙
∂ϕ dt
⎨
Lagrange’s
Equations
⎪
⎪
⎪ ∂L
⎪
⎪ =
Dynamics of ⎪
⎩ ∂ϕ
Open-chain
Manipulators
ml ¨ −ml sθ cθ ϕ˙
Coordinate
ml sθ
θ
¨ + ˙˙ + mglsθ =
Invariant ϕ ml sθ cθ θ ϕ
Algorithms
Lagrange’s
Equations with
Constraints

16. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
10
◻ Kinetic energy of a rigid body:
Chapter
Manipulator r
Dynamics B
Introduction
Lagrange’s
ra
Equations
A gab
Dynamics of
Open-chain
Manipulators
Coordinate Figure 4.5
Invariant
Algorithms Volume occupied by the body: V
Lagrange’s
Equations with Mass density: ρ(r)
m= ∫
Constraints
Mass: ρ(r)dV
V
Mass center: r≜
m ∫ V
ρ(r)rdV
Relative to frame at the mass center: r=
(Continues next slide)

17. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
11
Kinetic energy (In A-frame):
T= ρ(r) p + Rr dV = + pT Rr + Rr )dV
Chapter
Manipulator
Dynamics ∫V
˙ ˙ ∫ V
˙
ρ(r)( p ˙ ˙ ˙
= m p + pT R ∫ ρ(r)rdV + ∫
Introduction
˙ ˙ ˙ ˙
ρ(r) Rr dV
Lagrange’s V V
Equations
=
Dynamics of
Open-chain
Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

18. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
11
Kinetic energy (In A-frame):
T= ρ(r) p + Rr dV = + pT Rr + Rr )dV
Chapter
Manipulator
Dynamics ∫V
˙ ˙ ∫ V
˙
ρ(r)( p ˙ ˙ ˙
= m p + pT R ∫ ρ(r)rdV + ∫
Introduction
˙ ˙ ˙ ˙
ρ(r) Rr dV
Lagrange’s V V
Equations
=
Dynamics of
Open-chain
Manipulators
Coordinate
Invariant
Algorithms ∫
V
˙
ρ(r) Rr dV
= ∫ ρ(r) R Rr dV = ∫ ρ(r) ωr dV = ∫ ρ(r) ˆω dV
Lagrange’s
˙T
Equations with ˆ r
Constraints
= ∫ ρ(r)(−ω ˆ ω)dV = ω − ∫ ρ(r)ˆ dV ω ≜ ω Iω
r T T
r T
(Continues next slide)

19. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
12
⎡ Ixx Ixy Ixz ⎤
where
⎢ ⎥
I = − ρ(r)ˆ dV ≜ ⎢ Ixy Iyy Iyz ⎥
Chapter
∫ ⎢ ⎥
Manipulator
r
⎢ Ixz Iyz Izz ⎥
Dynamics
Introduction ⎣ ⎦
Lagrange’s
is the Inertia tensor with
Equations
Dynamics of
Open-chain
Ixx = ∫ ρ(r)(y + z )dxdydz, Ixy = − ∫ ρ(r)xydxdydz
Manipulators
Coordinate T= m p
˙ + (ωb )T Iωb = m RT p
˙ + (ωb )T Iωb
Invariant
Algorithms
Lagrange’s = (V b )T mI
I Vb
Equations with
Constraints
Mb
M b : Generalized inertia matrix in B-frame.

20. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
13
Example: Mb for a rectangular object
ρ=
Chapter m z
Manipulator
Dynamics lωh
Introduction Ixx = ∫ ρ(y + z )dxdydz y
V
Lagrange’s h ω l x
h
=ρ (y + z )dxdydz
Equations
Dynamics of ∫ ∫ ∫
− h
− ω
− l
l
Open-chain
w
=ρ (lω h + lωh ) = (ω + h )
Manipulators m
Coordinate Figure 4.6
Invariant
h ω l
Ixy = − ρxydV = −ρ
Algorithms
Lagrange’s
Equations with
∫ V
∫ ∫ ∫
−h −ω −l
xydxdydz
Constraints l
h ω
= −ρ dydz =
y
∫ ∫
−h −ω
x
−l
(Continues next slide)

21. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
14
⎡ (w + h ) ⎤
⎢ ⎥
m
⎢ ⎥ b
I=⎢ (l + h ) ⎥,M = mI
I
⎢ ⎥
m ×
⎢ (w + l ) ⎥
Chapter
⎣ ⎦
Manipulator
m
Dynamics
Introduction
Lagrange’s
Equations
Dynamics of
Open-chain
Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

22. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
14
⎡ (w + h ) ⎤
⎢ ⎥
m
⎢ ⎥ b
I=⎢ (l + h ) ⎥,M = mI
I
⎢ ⎥
m ×
⎢ (w + l ) ⎥
Chapter
⎣ ⎦
Manipulator
m
Dynamics
◻ M b under change of frames:
Introduction
Lagrange’s
Equations
Dynamics of V = g− ⋅ g , T = V T Mb V
ˆ ˙ g g (t)
Open-chain
V = Adg V
Manipulators
Coordinate
T = (Adg V )T M b (Adg V )
Invariant
Algorithms g (t)
g
Lagrange’s
= V T AdT M b Adg V ≜ V T M b V
Equations with
Constraints g
Figure 4.7
M b = AdT M b Adg
g

23. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
15
Example: Dynamics of a -dof planar robot
⎡ Ixxi ⎤
Chapter
⎢ ⎥
Manipulator
Ii = ⎢ Iyyi ⎥,i = ,
⎢ ⎥
Dynamics
Introduction
⎣ Izzi ⎦
l2
r 2 θ2
Lagrange’s
Equations
T(θ, θ) = m v + ωT I ω
Dynamics of
l1
Open-chain
Manipulators
˙ y
r1
θ1
+ m v + ω I ω
Coordinate
Invariant T x
Algorithms
Figure 4.8
Lagrange’s
Equations with
Constraints
ω = ω =
˙
θ θ +θ
˙ ˙
(Continues next slide)

24. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
16
xi
Chapter
Pi = yi ∶ Mass center
γi ∶ Distance from joint i to mass center
Manipulator
Dynamics
Introduction
Lagrange’s
Equations
Change of Coordinates:
⎧ x = −r s θ
⎪˙
x =rc ⎪
Dynamics of
˙
⇒⎨
Open-chain
y =rs ⎪y =r c θ
Manipulators
Coordinate ⎪˙
⎩
˙
Invariant
⎧ x = −(l s + r s )θ − r s θ
Algorithms
x =l c +r c ⎪˙
⎪ ˙ ˙
⇒⎨
Lagrange’s
y =l s +r s ⎪ y = (l c + r c )θ + r c θ
⎪˙
Equations with
⎩
Constraints ˙ ˙
(Continues next slide)

25. Chapter 4 Manipulator Dynamics
4.2 Lagrange’s Equations
17
Kinetic energy:
Chapter
Manipulator
T(θ, θ) = m (x + y ) + Iz θ + m (x + y ) + Iz (θ + θ )
˙ ˙ ˙ ˙ ˙ ˙ ˙ ˙
α + βc δ + βc
Dynamics
= [θ θ ]
˙
θ
δ + βc
Introduction ˙ ˙
Lagrange’s
δ ˙
θ
Equations
M(θ)
Dynamics of
Open-chain
α = Iz + Iz + m r + m (l + r ),
Manipulators
β = m l r , δ = Iz + m r , L = T
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with Equation of motion:
Constraints
−βs θ −βs (θ + θ )
+ =
¨
θ ˙ ˙ ˙ ˙
θ τ
M(θ) ¨ ˙ ˙ τ
θ βs θ θ
† End of Section †

26. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
18
◻ Dynamics of an open-chain manipulator:
Chapter Definition:
Li : frame at mass center of link i, gsli (θ) = e ξ ⋯e ξ i θ i gsli (o)
Manipulator ˆ θ ˆ
Dynamics
θ1
Introduction
θ2 θ3
l1 l2
Lagrange’s
L2 L3
Equations
Dynamics of r1 r2
Open-chain l0
L1
Manipulators
r0
Coordinate S
Invariant
Algorithms
Figure 4.9
Lagrange’s
⎡ θ ⎤
Equations with
⎢ ⎥
Constraints ˙
⎢ ⎥
⎢ ˙ ⎥
⎢ ⎥
Vsli = Jsli (θ)θ = [ξ † ξ † ⋯ ξ †
b b
⋯ ] ⎢ ˙θ i ⎥ = Ji (θ)θ
⎢ θ i+ ⎥
˙ ˙
⎢ ⎥
i
⎢ ⎥
⎢ ˙ ⎥
⎣ θn ⎦
(Continues next slide)

27. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
19
ξ † = Ad− (e ξ j+ ⋯e ξ i θ i gsli ( ))ξ j , j ≤ i
ˆ θ j+ ˆ
j
Ti (θ, θ) = (Vsli )T Mib Vsli = θ T JiT (θ)Mib Ji (θ)θ
Chapter ˙ b b ˙ ˙
Manipulator
Dynamics
n
Introduction T(θ) = Ti (θ, θ) =
˙ θ T M(θ)θ,
˙ ˙
i=
Lagrange’s
n
JiT (θ)Mib Ji (θ) = Mij (θ)θ i θ j
Equations
M(θ) = ˙ ˙
Dynamics of i i,j=
Open-chain
Manipulators
hi (θ): Height of Li , Vi (θ) = mi ghi (θ), V(θ) = mi ghi (θ)
Coordinate
i=
Invariant
Algorithms Lagrange’s Equation:
Lagrange’s
d ∂L ∂L
− = τ i , i = , . . . , n,
Equations with ˙
dt ∂θ i ∂θ i
Constraints
d ∂L d ⎛n ˙⎞
n
¨ ˙ ˙
Mij θ j = Mij θ j + Mij θ j
dt ⎝ j= ⎠
=
˙
dt ∂θ i j=
(Continues next slide)

28. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
20
∂L n ∂Mkj ∂V ∂Mij ˙
= ˙ ˙
θk θj − , ˙
Mij = θk
∂θ i j,k= ∂θ i ∂θ i k ∂θ k
Chapter
Manipulator n n ∂Mij ˙ ˙ ∂Mkj
⇒ ¨ ˙ ˙ ∂V
Dynamics Mij θ j + θj θk − θk θj + = τi
j= j,k= ∂θ k ∂θ i ∂θ i
Introduction
n n
⇒
Lagrange’s ¨ ˙ ˙ ∂V
Mij θ j + Γijk θ k θ j + = τi
Equations
j= j,k= ∂θ i
Dynamics of
Open-chain
∂Mij ∂Mik ∂Mkj
Γijk ≜ + −
Manipulators ∂θ k ∂θ j ∂θ i
Coordinate ˙ ˙
θ i ⋅ θ j , i ≠ j ∶ Coriolis term, ˙
θ i : Centrifugal term
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

29. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
20
∂L n ∂Mkj ∂V ∂Mij ˙
= ˙ ˙
θk θj − , ˙
Mij = θk
∂θ i j,k= ∂θ i ∂θ i k ∂θ k
Chapter
Manipulator n n ∂Mij ˙ ˙ ∂Mkj
⇒ ¨ ˙ ˙ ∂V
Dynamics Mij θ j + θj θk − θk θj + = τi
j= j,k= ∂θ k ∂θ i ∂θ i
Introduction
n n
⇒
Lagrange’s ¨ ˙ ˙ ∂V
Mij θ j + Γijk θ k θ j + = τi
Equations
j= j,k= ∂θ i
Dynamics of
Open-chain
∂Mij ∂Mik ∂Mkj
Γijk ≜ + −
Manipulators ∂θ k ∂θ j ∂θ i
Coordinate ˙ ˙
θ i ⋅ θ j , i ≠ j ∶ Coriolis term, ˙
θ i : Centrifugal term
Invariant
Algorithms
n n ∂Mij ∂Mik ∂Mkj ˙
Lagrange’s
Equations with
Define: Cij (θ, θ) =
˙ ˙
Γijk θ k = + − θk
k= k= ∂θ k ∂θ j ∂θ i
Constraints
⇒ M(θ)θ + C(θ, θ)θ + N(θ) = τ
¨ ˙ ˙

30. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
21
Chapter
Manipulator Property 1:
M(θ) = M T (θ), θ T M(θ)θ ≥ , θ T M(θ)θ = ⇔θ=
Dynamics ˙ ˙ ˙ ˙ ˙
Introduction ˙ − C ∈ ℝn×n is skew symmetric
M
Lagrange’s
Equations
Dynamics of
Open-chain
Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

31. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
21
Chapter
Manipulator Property 1:
M(θ) = M T (θ), θ T M(θ)θ ≥ , θ T M(θ)θ = ⇔θ=
Dynamics ˙ ˙ ˙ ˙ ˙
Introduction ˙ − C ∈ ℝn×n is skew symmetric
M
Lagrange’s
Equations Proof :
(M − C)ij = Mij − Cij (θ)
Dynamics of
Open-chain ˙ ˙
Manipulators
n ∂Mij ˙ ∂Mij ˙ ∂Mik ˙ ∂Mkj
Coordinate = θk − θk − θk + ˙
θk
Invariant k= ∂θ k ∂θ k ∂θ j ∂θ i
Algorithms
n ∂Mkj ∂Mik ˙
Lagrange’s = ˙
θk − θk
Equations with k= ∂θ i ∂θ j
Constraints
Switching i and j shows (M − C)T = −(M − C)
˙ ˙

32. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
22
Example: Planar 2-DoF Robot (continued)
Chapter
Manipulator m (θ) = α + β cos θ , m =δ
m (θ) = m (θ) = δ + β cos θ
Dynamics
Introduction
c (θ, θ) = −β sin θ ⋅ θ , c (θ, θ) = −β sin θ (θ + θ )
˙ ˙ ˙ ˙ ˙
Lagrange’s
Equations c (θ, θ) = β sin θ ⋅ θ , c (θ, θ) =
˙ ˙ ˙
Dynamics of
Open-chain
( )=
Manipulators ∂M ∂M ∂M ∂M
Γ = + − =
Coordinate ∂θ ∂θ ∂θ ∂θ
Invariant
( )=
Algorithms ∂M ∂M ∂M ∂M
Γ = + − = −β sin θ
Lagrange’s
∂θ ∂θ ∂θ ∂θ
( )=
Equations with ∂M ∂M ∂M ∂M
Constraints Γ = + − = −β sin θ
∂θ ∂θ ∂θ ∂θ
( )=
∂M ∂M ∂M ∂M ∂M
Γ = + − − = −β sin θ
∂θ ∂θ ∂θ ∂θ ∂θ
(Continues next slide)

33. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
23
( )=
∂M ∂M ∂M ∂M ∂M
Chapter Γ = + − − = β sin θ
Manipulator ∂θ ∂θ ∂θ ∂θ ∂θ
Dynamics
( )=
∂M ∂M ∂M ∂M
Γ = + − =
Introduction ∂θ ∂θ ∂θ ∂θ
( )=
Lagrange’s ∂M ∂M ∂M ∂M
Equations Γ = + − =
∂θ ∂θ ∂θ ∂θ
Dynamics of
( )=
Open-chain
∂M ∂M ∂M ∂M
Γ = + − =
Manipulators ∂θ ∂θ ∂θ ∂θ
Coordinate − β sin θ ⋅ θ˙ ˙
−β sin θ ⋅ θ
Invariant
˙
M− C=
−β sin θ ⋅ θ˙
Algorithms
Lagrange’s
−
˙
− β sin θ ⋅ θ − β sin θ (θ + θ )
˙ ˙
Equations with ˙
β sin θ ⋅ θ
Constraints
β sin θ ( θ + θ )
⇐ skew-symmetric
˙ ˙
−β sin θ ( θ + θ )
= ˙ ˙

34. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
24
Example: Dynamics of a -dof robot
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
⎢ ⎥ ⎢ ⎥ ⎢ −l ⎥
θ1
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ l ⎥
−l θ2 θ3
ξ =⎢ ⎥,ξ = ⎢ ⎥,ξ = ⎢ − ⎥
Chapter l1 l2
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
Manipulator
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
− L2 L3
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
Dynamics
⎣ ⎦ ⎣ ⎦ ⎣ ⎦ r1 r2
⎡ ⎤
Introduction
⎢ I ⎥
l0
gsl ( ) = ⎢ ⎥,
L1
⎢ ⎥
Lagrange’s
⎢ r ⎥
Equations r0
⎣ ⎦ S
Dynamics of
⎡ ⎤
⎢ ⎥
gsl ( ) = ⎢ I ⎥,
Open-chain r
⎢ ⎥ Figure 4.9
⎢ ⎥
Manipulators l
⎣ ⎦
⎡ ⎤
Coordinate
⎢ ⎥
Invariant
Algorithms
gsl ( ) = ⎢ I
⎢
l +r ⎥
⎥
⎢ l ⎥
Lagrange’s ⎣ ⎦
⎡ mi ⎤
Equations with
⎢ ⎥
⎢ ⎥
Constraints
mi
⎢ ⎥
Mi = ⎢ ⎥
mi
⎢ Ix i ⎥
⎢ Iy i ⎥
⎢ ⎥
⎢ Iz i ⎥
⎣ ⎦
(Continues next slide)

35. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
25
mi : The mass of the object
Chapter Ix i : The moment of inertia about the x axis
Manipulator
Dynamics
Introduction Γ = (Iy − Iz − m r )c s + (Iy − Iz )c s − t(l s + r s )
Lagrange’s Γ = (Iy − Iz )c s − tr s
Equations
Dynamics of
Γ = (Iy − Iz − m r )c s + (Iy − Iz )c s − t(l s + r s )
Open-chain Γ = (Iy − Iz )c s − tr s
Manipulators
Coordinate
Γ = (Iz − Iy + m r )c s + (Iz − Iy )c s + t(l s + r s )
Invariant Γ = −l m r s
Algorithms
Γ = −l m r s
Lagrange’s
Equations with Γ = −l m r s
= (Iz − Iy )c s
Constraints
Γ + tr s
Γ =lm r s
where t = m (l c + r c ).
(Continues next slide)

36. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
26
, V(θ) = m gh (θ) + m gh (θ) + m gh (θ)
˙ ∂V
N(θ, θ) =
∂θ
gsli (θ) = e ξ ⋯e ξ i θ i gsli ( ) ⇒
Chapter ˆ θ ˆ
Manipulator
Dynamics
h (θ) = r , h (θ) = l − r sin θ, h (θ)
Introduction
= l − l sin θ − r sin(θ + θ )
⎡ ⎤
⎢ ⎥
Lagrange’s
⎢ ⎥
⎢ ⎥
Equations
J = Jsl (θ) = ⎢
b
⎥
⎢ ⎥
⎢ ⎥
Dynamics of
⎢ ⎥
Open-chain
Manipulators ⎣ ⎦
⎡ −r c ⎤
⎢ ⎥
⎢ ⎥
Coordinate
Invariant
⎢ ⎥
J = Jsl (θ) = ⎢
b −r ⎥
⎢ ⎥
Algorithms
⎢ −s ⎥
−
⎢ c ⎥
⎣ ⎦
Lagrange’s
Equations with
⎡ −l c − r c ⎤
⎢ ⎥
Constraints
⎢ ⎥
⎢ ⎥
ls
J = Jsl (θ) = ⎢
b −r − l c ⎥
⎢ ⎥
−r
⎢ ⎥
− −
⎢ ⎥
⎣ ⎦
−s
c
(Continues next slide)

37. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
27
M M M
Chapter M(θ) = M M M = JT M J + JT M J + JT M J
Manipulator M M M
+ m r c + m (l c + r c )
Dynamics
M = Iy s + Iy s + Iz + Iz c + Iz c
Introduction
M =M =M =M =
Lagrange’s
Equations M = Ix + Ix + m l + M r + m r + m l r c
Dynamics of M = Ix + m r + m l r c
Open-chain
Manipulators M = Ix + m r + m l r c
Coordinate M = Ix + m r
Invariant
Algorithms n n ∂Mij ∂Mik ∂Mkj
Lagrange’s Cij (θ, θ) =
˙ ˙
Γijk θ k = + − ˙
θk
Equations with k= k= ∂θ k ∂θ j ∂θ i
Constraints

38. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
28
◻ Additional Properties of the dynamics in terms of POE:
⎧Ad−
Define:
⎪
⎪
⎪ e ξ j+ θ j+ ⋯e ξ i θ i i > j
Chapter
⎪
⎪
Manipulator ˆ ˆ
Aij = ⎨I
⎪
Dynamics
⎪
i=j
⎪
⎪
⎪
⎩
Introduction i<j
Lagrange’s
Equations
Ji (θ) = Adg − ( ) [Ai ξ ⋯Aii ξ i ⋯ ]
sl i
Dynamics of
Open-chain
Mi′ = AdT−
g
Mi Adg − ( intertia of ith link in S)
sl i ( ) sl i ( )
Manipulators
Coordinate
Invariant
Algorithms
Lagrange’s
Equations with
Constraints

39. Chapter 4 Manipulator Dynamics
4.3 Dynamics of Open-chain Manipulators
28
◻ Additional Properties of the dynamics in terms of POE:
⎧Ad−
Define:
⎪
⎪
⎪ e ξ j+ θ j+ ⋯e ξ i θ i i > j
Chapter
⎪
⎪
Manipulator ˆ ˆ
Aij = ⎨I
⎪
Dynamics
⎪
i=j
⎪
⎪
⎪
⎩
Introduction i<j
Lagrange’s
Equations
Ji (θ) = Adg − ( ) [Ai ξ ⋯Aii ξ i ⋯ ]
sl i
Dynamics of
Open-chain
Mi′ = AdT−
g
Mi Adg − ( intertia of ith link in S)
sl i ( ) sl i ( )
Manipulators
Coordinate
Invariant Property 2:
Algorithms n n ∂Mij ∂Mik ∂Mkj
Lagrange’s
Mij (θ) = ξ iT AT Ml′ Alj ξ j , Cij (θ, θ) =
li
˙ + − ˙
θk
Equations with l=max(i,j) k= ∂θ k ∂θ j ∂θ i
Constraints where
∂Mij n
= [Ak− ,i ξ i , ξ k ]T AT Ml′ Alj ξ j + ξ iT AT Ml′ Alk [Ak− ,j ξ j , ξ k ]
lk li
∂θ k l=max(i,j)
† End of Section †

40. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
29
◻ Newton-Euler equations in spatial frame:
Newton’s Equation:
f s = (mp) = mp
Chapter d
Manipulator ˙ ¨
Dynamics dt
Introduction Spatial angular momentum:
Lagrange’s
Equations
I s ⋅ ω s = R(I ⋅ ω b ) = R ⋅ I ⋅ RT ⋅ω s
Is
Dynamics of
Open-chain
Manipulators
Coordinate
Invariant
Algorithms
r
T
Lagrange’s
Equations with
Constraints
f
S
g ∶ (R, p)
Figure 4.10 (Continues next slide)

41. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
30
τs = (I ω ) = (RIRT ω s ) = I s ω s + RIRT ω s + RI RT ω s
d s s d
˙ ˙ ˙
dt dt
= I s ω s + RRT I s ω s − RIRT ω s ω s = I s ω s + ω s × (I s ω s )
Chapter
Manipulator
˙ ˙ ˆ ˙
Dynamics
Introduction ωs
ˆ
Lagrange’s
Equations
◻ Transform equations to body frame:
(mp) = mRvb = mRvb + mR˙ b , RT f s = mRT Rvb + m˙ b
Dynamics of d d ˙ ˙
Open-chain ˙ v v
Manipulators dt dt
Coordinate ⇒ f b = mω b × vb + m˙ b ,
v
Invariant
τ b = RT τ s = RT (RIω b ) = I ω b + ω b × Iω b
Algorithms d
˙
Lagrange’s dt
Equations with
Constraints
⇒ mI vb
˙ + ω b × mvb = fb = Fb (∗)
I ωb
˙ ω b × Iω b τb
Mb Vb
(Continues next slide)

42. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
31
Define:
[ , ] ∶ se( ) × se( ) ↦ se( ), [ ξ , ξ ] ≜ ξ ξ − ξ ξ
ˆ ˆ ˆˆ ˆ ˆ
Chapter
Manipulator if
[ξ , ξ ] = ωi
ˆ vi ,i = ,
Dynamics ˆ ˆ
Introduction
then
ξ= (ω × ω )∧ ω v −ω v = ad ξ i ⋅ ξ
Lagrange’s
Equations ˆ ˆ ˆ
Dynamics of
Open-chain where
ad ξ = ω
ˆ v
ˆ
Manipulators
Coordinate ωˆ
Invariant
Algorithms
It is straightforward computation to see that:
Lagrange’s (∗) ⇔ M b V b − adT b M b V b = F b
˙ V
Equations with
Constraints
Property 3:
Adg [ ξ , ξ ] = [Adg ξ , Adg ξ ] ⇒ Adg ad ξ = adAd g ξ Adg
ˆ ˆ ˆ ˆ ˆ ˆ

43. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
32
◻ Coordinate invariance of Newton-Euler equations:
Chapter
M V − adT M V = F
Manipulator
Dynamics ˙ V
⎧V = Adg V ,
r g
⎪
⎪
Introduction
g B
⎨ ⇒
⎪ F = AdT− F
Lagrange’s
⎪
⎩
Equations
g
F = (AdT )− F = (Ad− )T F
Dynamics of
Open-chain g
Manipulators g g A
Coordinate M = Ad−T M Ad−
g g Figure 4.11
Invariant
⇒ Adg M Ad−
−T
Adg V − ad(Ad g
T −T
Ad− Adg V = Ad−T F
Algorithms
˙
g V ) Adg M
˙ g g
Lagrange’s
Equations with
= Adg adV Ad− by Property 3, we have
Constraints
Since adAd g V g
M V − adT M V = F
˙ V

44. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
33
Chapter
Manipulator C2 C3
Dynamics
C1
Introduction
l0
Lagrange’s l1 l2
Equations C0
Dynamics of
Open-chain Figure 4.12
Manipulators
Ci : Frame fixed to link i, located along the ith axis
Coordinate
Invariant Fi : Generalized force link i − exerting on link i, expressed in Ci
Algorithms τi : Joint torque of link i
Lagrange’s
gi− ,i : Transformation of Ci relative to Ci−
gi− ,i (θ i ) = e ξ i θ i ⋅ gi− ,i ( ) = gi− ,i ( )e ξ i θ i
ˆ′ ˆ
Equations with
Constraints ′ th
ξ i = Adg − ( ) ⋅ ξ i : i axis in Ci frame.
⎧⎡ ⎤
i− ,i
⎪⎢
⎪⎢
⎪ ⎥
⎪ z ⎥∶
⎪⎢
⎪ ⎥
⎪
Revolute joint.
ξ i = ⎨⎣
i ⎦
⎡ z ⎤
⎪⎢ i ⎥
⎪
⎪⎢
⎪
⎪⎢ ⎥∶
⎪
⎪ ⎥ Prismatic joint.
⎩⎣ ⎦

45. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
34
Chapter ⇒ gi− ,i ⋅ gi− ,i = ξ i ⋅ θ i
−
˙ ˆ ˙
Manipulator
Dynamics
Mi : Moment of inertia in Ci
−mi ˆi
Mi =
Introduction
mi I r mi : Mass of link i
Lagrange’s
mi ˆi Ii − mi ˆi
r r Ii : inertia tensor
gi = gi− gi− ,i
Equations
Dynamics of
Vi = gi− ⋅ gi = gi− ,i Vi− gi− ,i + ξ i θ i
Open-chain
Manipulators ˆ ˙ − ˆ ˆ˙
Vi = Ad − Vi− + ξ i θ i ˙
Coordinate
Invariant gi− ,i
Algorithms
Lagrange’s Vi = gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i + ξ i θ i
˙ ˙− ˆ
ˆ − ˆ ˙ − ˙
ˆ ˆ¨
= −˙i− ,i gi− ,i gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i gi− ,i gi− ,i
Equations with
Constraints g− − ˆ − ˆ −
˙
+ gi− ,i Vi− gi− ,i + ξ i θ i
− ˙
ˆ ˆ¨
(Continues next slide)

46. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
35
= − ξ i θ i (Adg − Vi− )∧ + (Adg − Vi− )∧ ξ i θ i + (Adg − Vi− )∧ + ξ i θ i
ˆ˙ ˆ˙ ˙ ˆ¨
i− ,i i− ,i i− ,i
Chapter ⇒ Vi = ξ i θ i + Adg − Vi− − ad ξ
˙ ¨ ˙ ˙ (Adg − Vi− )
Manipulator i− ,i i θi i− ,i
Dynamics
Introduction ◻ Forward Recursion (kinematics):
⎧ init. ∶ V = , V = g (gravity vector)
⎪
⎪
Lagrange’s
⎪
˙
⎪
⎪
⎪
Equations
⎪
⎪g
⎪ i− ,i = gi− ,i ( )e ξ i θ i
⎪
⎪
ˆ
Dynamics of
⎨
⎪ Vi = Ad − Vi− + ξ i θ i
Open-chain
Manipulators ⎪
⎪
⎪
˙
⎪
⎪
⎪
g i− ,i
⎪ ˙
⎪ V = ξ θ + Ad − V − ad (Ad − V )
⎪
⎪
Coordinate
⎩
Invariant i
¨
i i ˙
g i− ,i i− ˙
ξi θ i g i− ,i i−
Algorithms
Lagrange’s
Equations with
Constraints
◻ Backward Recursion (inverse dynamics):
Fn+ : End-effector wrench, gn,n+ : transform from tool frame to Cn
Fi = AdT−
g
⋅ Fi+ + Mi Vi − adT i ⋅ Mi Vi
˙ V
i,i+
τ i = ξ iT ⋅ Fi

47. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
36
Chapter
Manipulator V = Adg −, ⋅ V + ξ θ
˙
Fn = AdTn,n+ ⋅ Fn+ + Mn Vn − adT n ⋅ (Mn Vn )
Dynamics
g−
˙ V
Introduction
Lagrange’s
Equations
Define:
⎡ θ ⎤
⎢ ˙ ⎥
˙=⎢ ⎥ ∈ ℝn , ξ =
Dynamics of
V ξ
V= ∈ℝ ,θ ⎢ ⎥ ⋱ ∈ℝ
Open-chain n× n×n
⎢ θn ⎥
Manipulators
Vn ⎣ ⎦
˙ ξn
⎡ Adg − ⎤
Coordinate
⎢ ⎥
Invariant
F τ ⎢ ⎥
F= ∈ℝ ,τ = ∈ ℝ ,P = ⎢ ⎥∈ℝ
Algorithms ,
⎢ ⎥
n× n n×
⎢ ⎥
Lagrange’s
Fn τn
⎣ ⎦
Equations with
Constraints
Pt = [ ⋯ Adgn,n+ ] ∈ ℝ
−
× n

48. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
37
˙
V = Adg − ⋅ V + ξ θ
,
Chapter
Manipulator ˙
V − Adg − V = ξ θ
Dynamics ,
Introduction
Vn − Adg − ˙
Vn− = ξ n θ n
Lagrange’s
n− ,n
Equations
⎡ ⎤
⎢ −Ad − ⎥⎡ V ⎤
I ⋯
⎢ ⎥⎢ V ⎥
Dynamics of
⇒⎢ ⎥⎢ ⎥
I ⋱
⎢ ⎥⎢ ⎥
Open-chain g,
⎢ ⋱ ⋱ ⎥⎢ ⎥
⎢ I ⎥ ⎣ Vn ⎦
Manipulators
⎣ ⎦
−Adg −
Coordinate n− ,n
Invariant V
Algorithms G−
⎡ Adg − ⎤ ⎡ ξ ⎤⎡ ⎤
⎢ ⎥ ⎢ ⎥⎢ ⎥
˙
⎥⎢ ⎥
θ
⎢ ⎥ ⎢
Lagrange’s
=⎢ ⎥V +⎢ ⎥⎢ ⎥
⎥⎢ ⎥
,
ξ ˙
θ
⎢ ⎥ ⎢
Equations with
⎢ ⎥ ⎢ ⎥⎢ ⎥
⋱
⎦⎢ ⎥
Constraints
⎣ ⎦ ⎣ ξn ⎣ ˙
θn ⎦
P ξ ˙
θ
˙
Thus V = GP V + Gξ θ

49. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
38
where
⎡ I ⋯ ⎤
Chapter
⎢ Ad − ⋯ ⎥
⎢ ⎥
Manipulator
⎢ ⎥
Dynamics I
G = ⎢ Adg −, ⎥∈ℝ
g,
Introduction
⎢ Adg −, I ⋱ ⎥
n× n
⎢ ⋱ ⎥
⎢ I ⎥
⎢ Adg − ⋯ Adgn− ,n I ⎥
Lagrange’s
⎣ Adg −,n ⎦
Equations −
,n
Dynamics of
Open-chain
Manipulators
Coordinate
Invariant
Algorithms V = ξ θ + Adg −, V − ad ξ θ (Adg −, V )
˙ ¨ ˙ ˙
Lagrange’s
Equations with
V − Adg −, V = ξ θ − ad ξ
˙ ˙ ¨ ˙
θ (Adg −, V )
Vn − Adgn− ,n Vn− = ξ n θ n − ad ξ n θ n (Adgn− ,n Vn− )
Constraints
˙ − ˙ ¨ ˙ −
(Continues next slide)

50. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
39
⎡ I ⋯ ⎤ ⎡ V ⎤ ⎡Ad − ⎤ ⎡ξ ⎤ ⎡θ ⎤
⎢ ⎥⎢ ˙ ⎥ ⎢ g , ⎥ ⎢ ⎥⎢¨ ⎥
¨
⎢−Adg −, I ⋱ ⎥ ⎢V ⎥ ⎢ ⎥˙ ⎢ ξ ⎥ ⎢θ ⎥
⎢ ⎥⎢ ˙ ⎥ = ⎢ ⎥V +⎢ ⎢ ⎥+
⎢ ⋱ ⋱ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⋱ ⎥⎢ ⎥
⎥⎢ ⎥
Chapter
⎢ −Adgn− ,n I ⎥ ⎢Vn ⎥ ⎢ ⎥ ⎢
⎢ ⎥⎣˙ ⎦ ⎣ ξ n ⎥ ⎢θ n ⎥
Manipulator
⎣ ⎦ ⎦ ⎣ ⎦⎣¨ ⎦
Dynamics −
Introduction
G− P ξ
⎡−ad ξ θ ⋯ ⎤ ⎡Ad −
Lagrange’s
⎢ ⎥⎢ g , ⎤
⎥
Equations
⎢ −ad ξ ⋱ ⎥⎢ ⎥
˙
⎢ ⎥⎢ ⎥V +
⎢ ⎥⎢
Dynamics of
⋱ ⋱ ⎥
˙
θ
⎢ ⎥
−ad ξ n θ n ⎥ ⎢ ⎥
Open-chain
⎢ ⋯ ˙ ⎦⎣ ⎦
Manipulators
Coordinate ⎣
Invariant
Algorithms ad ξ θ
˙
⎡−ad ξ θ ⋯ ⎤⎡ ⋯ ⎤ ⎡V ⎤
⎢ ⎥⎢ ⎥⎢ ⎥
Lagrange’s
⎢ −ad ξ ⋱ ⎥ ⎢Adg −, ⋱ ⎥ ⎢V ⎥
˙
⎢ ⎥⎢
Equations with
⎢ ⎥⎢ ⋱ ⋱ ⎥⎢ ⎥
⋱ ⋱ ⎥⎢ ⎥
Constraints ˙
θ
⎢ ⎥
⎢
⎣ ⋯ −ad ξ n θ n ⎥ ⎢
˙ ⎦⎣ Adgn− ,n
− ⎥ ⎣Vn ⎦
⎦
Γ
(Continues next slide)

51. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
40
Chapter
Thus
V = G ⋅ ξ θ + G ⋅ P V + G ⋅ ad ξ θ P V + G ⋅ ad ξ θ ΓV
Manipulator
Dynamics ˙ ¨ ˙ ˙ ˙
Introduction
Lagrange’s
Finally the backward recursion:
Fn = AdT− Fn+ + Mn Vn − adT n ⋅ (Mn Vn )
Equations
Dynamics of g
˙ V
n,n+
Fn− = Fn + Mn− Vn− − adT n− ⋅ (Mn− Vn− )
Open-chain
Manipulators
AdT−
gn−
˙ V
,n
Coordinate
Invariant
Algorithms
Lagrange’s F = AdT− F + M V − adT (M V )
g
˙ V
,
Equations with
Constraints
(Continues next slide)

52. Chapter 4 Manipulator Dynamics
4.4 Coordinate Invariant Algorithms
41
⎡ I−AdT− ⎤
⎢ ⎥ ⎡ F ⎤ ⎡M ⎤ ⎡V ⎤ ⎡ ⎤
⎢ ⎥ ⎢F ⎥ ⎢ M ⎥ ⎢V ⎥ ⎢ ⎥
˙
I ⋱ ⎥⎢ ˙ ⎥ +⎢ ⎥
g,
⎢
⇒⎢ ⎥⎢ ⎥ = ⎢
⎢
⎥⎢ ⎥ ⎢
⋱ ⋱−Adg − ⎥ ⎢ ⎥ ⎢ ⋱ ⎥ ⎢ ⎥ ⎢ T ⎥ Fn+ +
⎢ ⎥ ⎢ ⎥
Mn ⎥ ⎢V ⎥ ⎢Adgn,n+ ⎥
Chapter
T
⎢ n− ,n ⎥ ⎣F n ⎦ ⎣ ⎦ ⎣ ˙ n⎦ ⎣
Manipulator
Dynamics
⎣ ⋯ I ⎦
−
⎦
Introduction
M
PT
⎡−adT ⎤
Lagrange’s t
⎢ ⎥ M
⎢ ⎥
Equations
V
⎢ ⋱ ⋱
V
⎢ T ⎥
−adVn ⎥
Dynamics of
⎣ ⎦
Open-chain Mn Vn
Manipulators
Coordinate ad T
Invariant V
Algorithms
Lagrange’s
Equations with
Constraints