Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
Summer School-Math. Methods in [email protected] 13-31 July 2009
Chapter 4 Robot Dynamics and Control
1
Lecture Notes
for
AMathematical Introduction to
Robotic Manipulation
By
Z.X. Li∗ and Y.Q. Wu#
∗Dept. of ECE, Hong Kong University of Science & Technology#School of ME, Shanghai Jiaotong University
24 July 2009
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
Summer School-Math. Methods in [email protected] 13-31 July 2009
Chapter 4 Robot Dynamics and Control
2
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 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.1 Lagrangian EquationsChapter 4 Robot Dynamics and Control
3
◻ A Simple Example:
x
y
m
mg
FFx
Fy
♢ Review:Newton’s Equation: Lagrangian Equation:
mx = Fx
my = Fy −mg
Momentum: Px = mx
Py = myddtPx = Fx ,
ddtPy = Fy −mg
⇔
d
dt
∂L
∂x−∂L
∂x= Fx
d
dt
∂L
∂y−∂L
∂y= Fy
Lagrangian function:
L = T −V , Px =∂L∂x , Py =
∂L∂y
Kinetic energy:
T = 12m(x2 + y2)2
Potential energy:
V = mgy
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.1 Lagrangian EquationsChapter 4 Robot Dynamics and Control
4
◻ Generalization to multibody systems:
x
y
m1
q1
m2
q2
m3 q3
qi, i = 1, . . . , n: generalized coordinatesKinetic energy:
T = T(q, q)Potential energy:
V = V(q)Lagrangian:
L(q, q) = T(q, q) −V(q)τi, i = 1, . . . , n: external force on qiLagrangian Equation:
d
dt
∂L
∂qi−
∂L
∂qi= τi , i = 1, . . . , n
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.1 Lagrangian EquationsChapter 4 Robot Dynamics and Control
5
◇ Example: Pendulum equationGeneralized coordinate:
θ ∈ S1
Kinematics:
x = l sin θ , y = −l cos θ⇒ x = l cos θ ⋅ θ , y = l sin θ ⋅ θ
Kinetic energy:
T(θ , θ) =1
2m(x2 + y2) = 1
2ml2θ2
Potential energy:
V = mgl(1 − cos θ)
x
y
mg
θ
Lagrangian function:
L = T −V = 1
2ml2θ −mgl(1 − cos θ),⇒ ∂L
∂θ= ml2θ ,
∂L
∂θ= −mgl sin θ
Equation of motion:d
dt
∂L
∂θ− ∂L
∂θ= τ⇒ ml2θ +mgl sin θ = τ
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.1 Lagrangian EquationsChapter 4 Robot Dynamics and Control
6
◇ Example: Dynamics of a Spherical Pendulum
r(θ , ϕ) =⎡⎢⎢⎢⎣l sin θ cos ϕl sin θ sin ϕ−l cos θ
⎤⎥⎥⎥⎦T =
1
2m∥r∥2 = 1
2ml2(θ2 + (1 − cos2 θ)ϕ2)
V = −mgl cos θ
L(q, q) = 1
2ml2(θ + (1 − cosθ)ϕ2) +mgl cos θ
θ
ϕ mg
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.1 Lagrangian EquationsChapter 4 Robot Dynamics and Control
7
⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩
d
dt
∂L
∂θ=
d
dt(ml2θ) = ml2θ ,
∂L
∂θ= ml2 sin θ cos θϕ2 −mgl sin θ
d
dt
∂L
∂ϕ=
d
dt(ml2 sin2 θϕ) = ml2 sin2 θϕ + 2ml2 sin θ cos θθϕ,
∂L
∂ϕ= 0
[ ml2 00 ml2s2θ
] [ θϕ] + [ −ml2sθcθ ϕ
2
2ml2sθcθ θ ϕ] + [ mglsθ
0 ] = [ 00 ]
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.2 Intertial Properties of Rigid BodyChapter 4 Robot Dynamics and Control
8
◻ Kinetic energy of a rigid body:
A
Br
gab
ra
Volume occupied by the body: VMass density: ρ(r)
Mass: m = ∫Vρ(r)dV
Mass center: r ≜ 1
m ∫Vρ(r)rdV
Relative to frame at the mass center: r = 0
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.2 Intertial Properties of Rigid BodyChapter 4 Robot Dynamics and Control
9
In A-frame
T =1
2 ∫V ρ(r)∥p + Rr∥2dV = 1
2 ∫V ρ(r)(∥p∥2 + 2pT Rr + ∥Rr∥2)dV=1
2m∥p∥2 + pTR ∫
Vρ(r)rdV
´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶=0+ 12 ∫ ρ(r)∥Rr∥2dV
1
2 ∫V ρ(r)∥Rr∥2dV=1
2 ∫ ρ(r)∥RT Rr∥2dV = 1
2 ∫ ρ(r)∥ωr∥2dV = 1
2 ∫ ρ(r)∥rω∥2dV=1
2 ∫ ρ(r)(−ωT r2ω)dV = 1
2ωT ( ∫ (−ρ(r)r)2dV)ω ≜ 1
2ωT
Iω
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.2 Intertial Properties of Rigid BodyChapter 4 Robot Dynamics and Control
10
where
I = − ∫ ρ(r)r2dV ≜⎡⎢⎢⎢⎢⎣Ixx Ixy IxzIxy Iyy IyzIxz Iyz Izz
⎤⎥⎥⎥⎥⎦with
Ixx = ∫ ρ(r)(y2 + z2)dxdydz, Ixy = − ∫ ρ(r)xydxdydz
T =1
2m∥p∥2 + 1
2(ωb)TIbωb =
1
2m∥RT p∥2 + 1
2(ωb)TIbωb
=1
2(Vb)T [ mI 0
0 Ib ]
´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶Mb
Vb
For Vb = g−1 ⋅ g, with ωb = RT ⋅ R and vb = RT p,Mb is the
Generalized inertia matrix in B frame.
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.2 Intertial Properties of Rigid BodyChapter 4 Robot Dynamics and Control
11
◇ Example: Mb for a rectangular object
ρ =m
lωhIxx = ∫
Vρ(y2 + z2)dxdydz
= ρ ∫h2
− h2
∫ω2
− ω2
∫l2
− l2
(y2 + z2)dxdydz= ρ ( 1
12(lω3h + lωh3)) = m
12(ω2 + h2)
Ixy = − ∫Vρxydv = −ρ ∫
h2
− h2
∫ω2
− ω2
∫l2
− l2
xydxdydz
= −ρ ∫h2
− h2
∫ω2
− ω2
y
2x2∣
l2
− l2
dydz = 0
x
y
z
h
w
l
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.2 Intertial Properties of Rigid BodyChapter 4 Robot Dynamics and Control
12
I =
⎡⎢⎢⎢⎢⎢⎣
m12(w2 + h2) 0 0
0 m12(l2 + h2) 0
0 0 m12(w2 + l2)
⎤⎥⎥⎥⎥⎥⎦,M = [ mI3×3 0
0 I]
V1 = g−11 ⋅ g1 , M1
T =1
2VT1 M1V1
V1 = Adg0V2
A B
g0
g1(t)
g2(t)
g0
T =1
2(Adg0V2)TM1(Adg0V2) = 1
2VT2 Ad
Tg0M1Adg0V2 ≜ 1
2VT2 M2V2
◻M under change of frames:
M2 = AdTg0M1Adg0
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.2 Intertial Properties of Rigid BodyChapter 4 Robot Dynamics and Control
13
◇ Example: Dynamics of a 2-dof planar robot
Ii =⎡⎢⎢⎢⎣Ixxi 0 00 Iyyi 00 0 Izzi
⎤⎥⎥⎥⎦ , i = 1, 2
T(θ , θ) = 1
2m1∥v1∥2 + 1
2ωT1 I1ω1
+ 1
2m2∥v2∥2 + 1
2ωT2 I2ω2
ω1 = [ 00θ1] ω2 = [ 0
0θ1 + θ2 ]
θ1
l1
θ2
l2
r1
r2
x
y
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.2 Intertial Properties of Rigid BodyChapter 4 Robot Dynamics and Control
14
Pi = [ xiyi0]: Mass center γi: Distance from joint i to mass center
x1 = r1c1
y1 = r1s1⇒
x1 = −r1s1 θ1y1 = r1c1θ1
x2 = l1c1 + r2c12y2 = l1s1 + r2s12 ⇒
x2 = −(l1s1 + r2s12)θ1 − r2s12θ2y1 = (l1c1 + r2c12)θ1 + r2c12θ2
T(θ , θ) = 1
2m1(x21 + y21 ) + 1
2Iz1 θ
21 + 1
2m2(x22 + y22) + 1
2Iz2(θ21 + θ22)
=1
2[θ1 θ2] [ α + 2βc2 δ + βc2
δ + βc2 δ ]´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶
M(θ)[ θ1θ2]
α = Iz1 + Iz2 +m1r21 +m2(l21 + r22), β = m2l1r2 , δ = Iz2 +m2r
22 , L = T
⇒M(θ) [ θ1θ2] + [ −βs2θ2 −βx
βs2θ1 0] [ θ1
θ2] = [ τ1
τ2 ]
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
15
◻Dynamics of open-chain manipulator:
De�nition:
Li: frame at mass center of link i, gsli(θ) = expξ1θ1 ⋯ expξiθ i gsli(o)θ1
θ2
l0
θ3
l1 l2
r2r1
S
r0
L1
L2 L3
Vbsli= Jbsli(θ)θ = [ξ†1 ξ†2 ⋯ ξ†i 0⋯ 0]
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣
θ1⋮θiθi+1⋮θn
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦= Ji(θ)θ
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
16
ξ†j = Ad−1(eξj+1θ j+1⋯eξiθ igsli(0))ξj , j ≤ i
Ti(θ , θ) = 1
2(Vb
sli)TMb
i Vbsli=1
2θTJTi (θ)Mb
i Ji(θ)θT(θ) = n∑
i=1Ti(θ , θ) = 1
2θTM(θ)θ ,
M(θ) =∑i
JTi (θ)Mbi Ji(θ) = 1
2
n∑i,j=1
Mij(θ)θi θjhi(θ): Height of Li, Vi(θ) = mighi(θ), V(θ) =∑
i=1mighi(θ)
Lagrange’s Equation:
d
dt
∂L
∂θi− ∂L
∂θi= Γi, i = 1, . . . , n,
d
dt
∂L
∂θi=
d
dt
⎛⎝
n∑j=1
Mijθj⎞⎠ =
n∑j=1
Mijθj + Mijθj
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
17
∂L
∂θi=1
2
n∑j,k=1
∂Mkj
∂θiθkθj − ∂V
∂θiMij =∑
k
∂Mij
∂θkθk
⇒n∑j=1
Mijθj + n∑j,k=1(∂Mij
∂θkθjθk − 1
2
∂Mkj
∂θiθkθj) + ∂V
∂θi= Γi
⇒n∑j=1
Mijθj + n∑j,k=1
Γkij θkθj + ∂V
∂θi= Γi
Γkij =1
2(∂Mij
∂θk+ ∂Mik
∂θj− ∂Mkj
∂θi)
θi ⋅ θj , i ≠ j : Coriolis force θ2i : Centrifugal force
De�ne: cij(θ , θ) = n∑k=1
Γkij θk =1
2
n∑k=1(∂Mij
∂θk+ ∂Mik
∂θj− ∂Mkj
∂θi)θk
⇒ M(θ)θ +C(θ , θ)θ +N(θ) = τ
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
18
Property 1:
1 M(θ) =MT(θ), θTM(θ)θ ≥ 0, θTM(θ)θ = 0⇔ θ = 0
2 M − 2C ∈ Rn×n is skew symmetric
Proof :
(M − 2C)ij = Mij − 2cij(θ)=
n∑k=1
∂Mij
∂θkθk − ∂Mij
∂θkθk − ∂Mik
∂θjθk + ∂Mkj
∂θiθk
=n∑k=1
∂Mkj
∂θiθk − ∂Mik
∂θjθk
Switching i and j shows (M − 2C)T = −(M − 2C)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
19
◇ Example: Planar 2-DoF Robot (continued)m11(θ) = α + 2β cos θ2,m22 = δ
m12(θ) = m21(θ) = δ + β cos θ2c11(θ , θ) = −β sin θ2 ⋅ θ2, c12(θ , θ) = −β sin θ2(θ1 + θ2)c21(θ , θ) = β sin θ2 ⋅ θ1 , c22(θ , θ) = 0⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩
Γ111 =1
2(∂M11
∂θ1+ ∂M11
∂θ1− ∂M11
∂θ1) = 1
2
∂M11
∂θ1= 0
Γ211 =1
2(∂M11
∂θ2+ ∂M12
∂θ1− ∂M21
∂θ1) = 1
2
∂M11
∂θ2= −β sin θ2
Γ112 =1
2(∂M12
∂θ1+ ∂M11
∂θ2− ∂M12
∂θ1) = 1
2
∂M11
∂θ2= −β sin θ2
Γ212 =1
2(∂M12
∂θ2+ ∂M12
∂θ2− ∂M22
∂θ1) = ∂M12
∂θ2− 1
2
∂M22
∂θ1= −β sin θ2
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
20
⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩
Γ121 =1
2(∂M21
∂θ1+ ∂M21
∂θ1− ∂M11
∂θ2) = ∂M21
∂θ1− 1
2
∂M11
∂θ2= β sin θ2
Γ221 =1
2(∂M21
∂θ2+ ∂M22
∂θ1− ∂M21
∂θ2) = 1
2
∂M22
∂θ1= 0
Γ122 =1
2(∂M22
∂θ1+ ∂M21
∂θ2− ∂M12
∂θ2) = 1
2
∂M22
∂θ1= 0
Γ222 =1
2(∂M22
∂θ2+ ∂M22
∂θ2− ∂M22
∂θ2) = 1
2
∂M22
∂θ2= 0
M − 2C = [ −2β sin θ2 ⋅ θ2 −β sin θ2 ⋅ θ2−β sin θ2 ⋅ θ2 0]
− [ −2β sin θ2 ⋅ θ2 −2β sin θ2(θ1 + θ2)2β sin θ2 ⋅ θ1 0
]= [ 0 β sin θ2(2θ1 + θ2)−β sin θ2(2θ1 + θ2) 0
] ⇐ skew-symmetric
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
21
◇ Example: Dynamics of a 3-dof robot
ξ1 =⎡⎢⎢⎢⎢⎢⎢⎢⎣
000001
⎤⎥⎥⎥⎥⎥⎥⎥⎦, ξ2 =
⎡⎢⎢⎢⎢⎢⎢⎢⎣
0−l00−101
⎤⎥⎥⎥⎥⎥⎥⎥⎦, ξ3 =
⎡⎢⎢⎢⎢⎢⎢⎢⎣
0−l0l1−101
⎤⎥⎥⎥⎥⎥⎥⎥⎦
θ1
θ2
l0
θ3
l1 l2
r2r1
S
r0
L1
L2 L3
gsl1(0) =⎡⎢⎢⎢⎢⎣
I ( 00r0)
0 1
⎤⎥⎥⎥⎥⎦, gsl2 (0) =
⎡⎢⎢⎢⎢⎣I ( 0
r1l0)
0 1
⎤⎥⎥⎥⎥⎦, gsl3 (0) =
⎡⎢⎢⎢⎢⎣I ( 0
l1 + r2l0
)0 1
⎤⎥⎥⎥⎥⎦
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
22
Mi =
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣
mi 0 00 mi 00 0 mi
0
0Ixi 0 00 Iyi 00 0 Izi
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦mi:�e mass of the objectIxi :�e moment of inertia about the x axis
Γ211 = (Iy2 − Iz2 −m2r21 )c2s2 + (Iy2 − Iz3)c23s23 −m3(l1c2 + r2c23)(l1s2 + r2s23)Γ311 = (Iy3 − Iz3)c23s23 −m3r2s23(l1c2 + r2c23)Γ112 = (Iy2 − Iz2 −m2r
21 )c2s2 + (Iy3 − Iz3)c23s23 −m3(l1c2 + r2c23)(l1s2 + r2s23)
Γ113 = (Iy3 − Iz3)c23s23 −m3r2s23(l1c2 + r2c23)Γ121 = (Iz2 − Iy2 +m2r
21 )c2s2 + (Iz3 − Iy3)c23s23 +m3(l1c2 + r2c23)(l1s2 + r2s23)
Γ322 = −l1m3r2s3 , Γ223 = −l1m3r2s3 , Γ323 = −l1m3r2s3Γ131 = (Iz3 − Iy3)c23s23 +m3r2s23(l1c2 + r2c23) , Γ232 = l1m3r2s3
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
23
N(θ , θ) = ∂V
∂θ, V(θ) = m1gh1(θ) +m2gh2(θ) +m2gh3(θ)
gsli(θ) = eξ1θ1⋯eξiθ igsli(0)⇒h1(θ) = r0 , h2(θ) = l0 − r1 sin θ , h3(θ)
= l0 − l1 sin θ2 − r2 sin(θ2 + θ3)gsli(θ)= eξ1θ1⋯eξiθ igsli(0)⇒
h1(θ) = r0 , h2(θ) = l0 − r1 sin θ , h3(θ)= l0 − l1 sin θ2 − r2 sin(θ2 + θ3)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
24
J1 = Jbsl1(θ) =
⎡⎢⎢⎢⎢⎢⎢⎢⎣
0 0 00 0 00 0 00 0 00 0 01 0 0
⎤⎥⎥⎥⎥⎥⎥⎥⎦J2 = J
bsl2(θ) =
⎡⎢⎢⎢⎢⎢⎢⎢⎣
−r1c2 0 00 0 00 −r1 00 −1 0−s2 0 0c2 0 0
⎤⎥⎥⎥⎥⎥⎥⎥⎦
J3 = Jbsl3(θ) =
⎡⎢⎢⎢⎢⎢⎢⎢⎣
−l2c2 − r2c23 0 00 l1s3 00 −r2 − l1c3 −r20 −1 −1−s23 0 0c23 0 0
⎤⎥⎥⎥⎥⎥⎥⎥⎦
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
25
M(θ) = [ M11 M12 M13M21 M22 M23M31 M32 M33
] = JT1 M1J1 + JT2 M2J2 + JT3 M3J3
M11 = Iy2s22 + Iy3s223 + Iz1 + Iz2c22 + Iz3c223 +m2r
21 c
22 +m3(l1c2 + r2c23)2
M12 =M13 =M21 =M31 = 0
M22 = Ix2 + Ix3 +m2l21 +M2r
21 +m3r
22 + 2m3l1r2c3
M23 = Ix3 +m3r22 +m3l1r2c3
M32 = Ix3 +m3r22 +m3l1r2c3
M33 = Ix3 +m3r22
Cij(θ , θ) = n∑k=1
Γkij θk =1
2
n∑k=1(∂Mij
∂θk+ ∂Mik
∂θj− ∂Mkj
∂θi) θk
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
26
◻ Additional Properties of the dynamics interms of POE:De�ne:
Aij =
⎧⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎩
Ad−1eξj+1 θj+1⋯e ξi θi i > j
I i = j
0 i < jJi(θ) = Adg−1sli
(0)[Ai1ξ1⋯Aiiξi 0⋯0]M′i = AdTg−1
sli(0)MiAdg−1
sli(0)( intertia of ith link in S)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.3 Dynamics of Open-chain ManipulatorChapter 4 Robot Dynamics and Control
27
Property 2:
Mij(θ) = n∑l=max(i,j)
ξTi ATliM′lAljξj
Cij(θ , θ) = 1
2
n∑k=1(∂Mij
∂θk+ ∂Mik
∂θj− ∂Mkj
∂θi) θk
where∂Mij
∂θk=
n∑l=max(i,j)
([Ak−1,i ξi, ξk]TATlkM
′lAljξj
+ξTi ATliM′lAlk[Ak−1,j ξj, ξk])
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.4 Newton-Euler EquationsChapter 4 Robot Dynamics and Control
28
◻ Newton-Euler equations in body frame:Newtons Equation:
f =d
dt(mp) = mp
Spatial angular momentum:
I′ ⋅ ωs = R(I ⋅ ωb) = R ⋅ I ⋅ RT´¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¶I′
⋅ωs
S
Tr
g ∶ (R, p)f
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.4 Newton-Euler EquationsChapter 4 Robot Dynamics and Control
29
τ =d
dt(I sωs) = d
dt(RIbRTωs) = I sωs + RIbRTωs + RIbRTωs
= I sωs + RRT±ωs
Isωs − RIbRT ωsωs = I sωs + ωs × (I sωs)
◻ Transformation of all equations totwist/wrench in body frame:
d
dt(mp) = d
dt(mRvb) = mRvb +mRvb,RT f s = mRTRvb +mvb
⇒ f b = mωb × vb +mvb,
τb = RTτs = RT d
dt(RIbωb) = Ibωb + ωb × Ibωb
⇒ [ mI 00 I
b ]´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶
M
[ vb
ωb ]´¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¶
Vb
+ [ ωb ×mvb
ωb × Ibωb ] = [ f b
τb] = Fb(∗)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.4 Newton-Euler EquationsChapter 4 Robot Dynamics and Control
30
De�ne: se(3) × se(3) ↦ se(3), (ξ1 , ξ2)↦ ξ1 ξ2 − ξ2 ξ1 ≜ ξ, ifξi = [ ωi vi
0 0 ] , i = 1, 2then
ξ = [ (ω1 × ω2)∧ ω1v2 − ω2v10 0 ] = adξi ⋅ ξ2
where
adξ1 = [ ω1 v10 ω1
]�us (∗)⇔MbVb − adVbMbVb = F
b
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.4 Newton-Euler EquationsChapter 4 Robot Dynamics and Control
31
◻ Coordinate invariance of Newton-Eulerequations:
M1V1 − adTV1M1V1 = F1
V1 = Adg0V2
AdTg0F1 = F2⇒
F1 = (AdTg0)−1F2 = (Ad−1g0 )TF2 A
Br
g1
g2
g0
M1 = Ad−Tg0 M2Ad
−1g0
⇒ Ad−Tg0 M2Ad−1g0Adg0 V2 − adT(Adg0 V2)Ad−Tg0 M2Ad
−1g0Adg0V1 = Ad
−Tg0 F2
Since adAdg0V = Adg0adVAd−1g0 , we have
M2V2 − adV2M2V2 = F2
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
32
C0
C1
C2
Cn Cn+1
Ci: Frame �xed to link i, located along the ith axisFi: Generalized force link i − 1 exerting on link i, expressed in Ciτi: Joint torque of link i
gi−1,i: Transformation of Ci relative to Ci−1gi−1,i(θi) = eξ′i θ i ⋅ gi−1,i(0) = gi−1,i(0)eξiθ i
ξi = Adg−1i−1,i(0) ⋅ ξ′i : ith axis in Ci frame.
ξ =
⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎩
⎡⎢⎢⎢⎢⎣0
zi
⎤⎥⎥⎥⎥⎦∶ Revolute joint.
⎡⎢⎢⎢⎢⎣zi
0
⎤⎥⎥⎥⎥⎦∶ Prismatic joint.
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
33
⇒ g−1i−1,i ⋅ gi−1,i = ξi ⋅ θiMi: Moment of inertia in Ci
Mi = [ miI −mirimiri Ii −mir
2i] mi: Mass of link i
Ii: Angular moment of inertia
gi = gi−1gi−1,iVi = g
−1i ⋅ gi = g−1i−1,iVi−1gi−1,i + ξiθi
Vi = Adg−1i−1,iVi−1 + ξiθi
˙Vi = g−1i−1,iVi−1gi−1,i + g−1i−1,iVi−1gi−1,i + g−1i−1,i ˙Vi−1gi−1,i + ξiθi
= −g−1i−1,i gi−1,ig−1i−1,iVi−1gi−1,i + g−1i−1,iVi−1gi−1,ig−1i−1,i gi−1,i+ g−1i−1,i ˙Vi−1gi−1,i + ξiθi
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
34
= −ξiθi(Adg−1i−1,iVi−1)∧ + (Adg−1i−1,i
Vi−1)∧ ξiθi + (Adg−1i−1,iVi−1)∧ + ξiθi
⇒ Vi = ξiθi +Adg−1i−1,iVi−1 − adξi θ i(Adg−1i−1,i
Vi−1)◻ Forward Recursion:i = 0 ∶ V0 = 0, V0 = [ g
0 ]⎧⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎩
gi−1,i = gi−1,i(0)eξiθ iVi = Adg−1i−1,i
Vi−1 + ξiθiVi = ξiθi +Adg−1i−1,i
Vi−1 − adξi θ i(Adg−1i−1,iVi−1)
◻ Backward Recursion:Fn+1: End-e�ector wrench, gn,n+1: transform from tool frame to Cn
Fi = AdTg−1i,i+1⋅ Fi+1 +MiVi − adTVi
⋅MiVi
τi = ξTi ⋅ Fi
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
35
V1 = Adg−10,1⋅V0 + ξ1θ1
Fn = AdTg−1n ,n+1⋅ Fn+1 +MnVn − adTVn
⋅ (MnVn)De�ne:
V = [ V1⋮Vn] ∈ R
6n×1, θ =⎡⎢⎢⎢⎢⎣
θ1⋮θn
⎤⎥⎥⎥⎥⎦∈ R
n, ξ = [ ξ1 0 00 ⋱ 00 0 ξn
] ∈ R6n×n
F = [ F1⋮Fn] ∈ R
6n×1, τ = [ τ1⋮τn] ∈ R
n, P0 =
⎡⎢⎢⎢⎢⎢⎣Adg−10,1
0⋮0
⎤⎥⎥⎥⎥⎥⎦∈ R
6n×6
Pt = [0⋯0 AdTg−1n ,n+1] ∈ R
6×6n
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
36
V1 = Adg−10,1⋅V0 + ξ1θ1
V2 −Adg−11,2V1 = ξ2θ2
⋮
Vn −Adg−1n−1,nVn−1 = ξnθn
⇒
⎡⎢⎢⎢⎢⎢⎢⎣
I 0 ⋯ 0−Adg−11,2
I ⋱ ⋮
0 ⋱ ⋱ 00 0 −Adg−1n−1,n
I
⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶G−1
⎡⎢⎢⎢⎢⎣V1V2⋮Vn
⎤⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¶V
=
⎡⎢⎢⎢⎢⎢⎣Adg−10,1
0⋮0
⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶P0
V0 +
⎡⎢⎢⎢⎢⎢⎣ξ1
ξ2⋱
ξn
⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ξ
⎡⎢⎢⎢⎢⎢⎢⎣
θ1θ2⋮
θn
⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¶θ
�us V = GP0V0 +Gξθ
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
37
where
G =
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣
I 0 0 ⋯ 0Adg−11,2
I 0 ⋯ 0
Adg−11,3Adg−12,3
I ⋱ ⋮
⋮ ⋮ ⋱ I 0Adg−11,n
Adg−12,n⋯ Adg−1n−1,n
I
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦∈ R
6n×6n
V1 = ξ1θ1 +Adg−10,1V0 − adξ1 θ1(Adg−10,1
V0)V2 −Adg−11,2
V1 = ξ2θ2 − adξ2 θ2(Adg−11,2V1)
Vn −Adg−1n−1,nVn−1 = ξnθn − adξn θn(Adg−1n−1,n
Vn−1)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
38
⎡⎢⎢⎢⎢⎢⎢⎣
I 0 ⋯ 0−Adg−11,2I ⋱ ⋮
0 ⋱ ⋱ 00 0 −Adg−1n−1,n
I
⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶G−1
⎡⎢⎢⎢⎢⎢⎣V1V2⋮Vn
⎤⎥⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎢⎣
Adg−10,1
0⋮0
⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶P0
V0+⎡⎢⎢⎢⎢⎢⎣
ξ1ξ2 ⋱
ξn
⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ξ
⎡⎢⎢⎢⎢⎢⎢⎣θ1θ2⋮θn
⎤⎥⎥⎥⎥⎥⎥⎦
+⎡⎢⎢⎢⎢⎢⎢⎣
−adξ1 θ 1 0 ⋯ 00 −adξ2 θ2 ⋱ ⋮⋮ ⋱ ⋱ 00 ⋯ 0 −adξn θn
⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ad
ξ θ
⎡⎢⎢⎢⎢⎢⎣Adg−10,10⋮0
⎤⎥⎥⎥⎥⎥⎦V0
+⎡⎢⎢⎢⎢⎢⎢⎣
−adξ1 θ 1 0 ⋯ 00 −adξ2 θ2 ⋱ ⋮⋮ ⋱ ⋱ 00 ⋯ 0 −adξn θn
⎤⎥⎥⎥⎥⎥⎥⎦
⎡⎢⎢⎢⎢⎢⎢⎣
0 0 ⋯ 0Adg−11,2
0 ⋱ ⋮0 ⋱ ⋱ 00 0 Adg−1n−1,n
0
⎤⎥⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶Γ
⎡⎢⎢⎢⎢⎣V1V2⋮Vn
⎤⎥⎥⎥⎥⎦
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
39
�us
V = G ⋅ ξθ +G ⋅ P0V0 +G ⋅ adξθP0V0 +G ⋅ adξθΓV
Finally the backward recursion:
Fn = AdTg−1n ,n+1
Fn+1 +MnVn − adTVn⋅ (MnVn)
Fn−1 = AdTg−1n−1,nFn +Mn−1Vn−1 − adTVn−1
⋅ (Mn−1Vn−1)⋮
F1 = AdTg−11,2
F2 +M1V1 − adTV1(M1V1)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
40
⇒
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣
I −AdTg−11,20 0
0 I ⋱ 0⋮ ⋱ ⋱ −AdTg−1n−1,n0 ⋯ 0 I
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦
⎡⎢⎢⎢⎢⎢⎢⎣
F1F2⋮Fn
⎤⎥⎥⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎢⎢⎣
M1M2
⋱Mn
⎤⎥⎥⎥⎥⎥⎥⎦´ ¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸ ¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶M
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣
V1V2⋮
Vn
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦
+
⎡⎢⎢⎢⎢⎢⎣
0⋮0
AdTg−1n ,n+1
⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶PTt
Fn+1 +⎡⎢⎢⎢⎢⎣−adTV1
⋱
−adTVn
⎤⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶adTV
[ M1⋱
Mn][ V1⋮Vn]
F = GTMV +GTPTt Fn+1±Ft
+GT⋅ adTVMV
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
41
⇒
τ = ξT ⋅ F
τ = ξTGTMV + ξTGTPTt Ft + ξTGT⋅ adTVMV
= ξTGTM(Gξθ +GP0V0 +G ⋅ adξθP0V0 +G ⋅ adξθΓV)+ ξTGTPTt Ft + ξ
TGT⋅ adTVMV
= ξTGTMGξθ + ξTGTMGP0V0 + ξTGTMG ⋅ adξθΓV
+ ξTGTPTt Ft + ξTGT⋅ adTVMV
Finally we get:M(θ)³¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹·¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹µ
ξTGTMGξ θ +
C(θ ,θ)³¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹·¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹µξTGT(MG ⋅ adξθΓ + ad
TVM)Gξ θ
+ ξTGTMGP0V0´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ϕ(θ)
+ ξTGTPTt´¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¶JTt (θ)
Ft = τ
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.5 Coordinate-invariant algorithms for robot dynamics
Chapter 4 Robot Dynamics and Control
42
M(θ)θ + C(θ , θ) + ϕ(θ) + JTt (θ)Ft = τM(θ) = ξTGTMGξ
C(θ , θ) = ξTGT(MGadξθΓ + adTVM)Gξθ
ϕ(θ) = ξTGTMGP0V0
Jt = PtGξProperty 3:
Γn = 0,
G = (I − Γ)−1 = I + Γ + Γ2 +⋯+ Γn−1I + ΓG = G
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
43
De�nition: Holonomic constraints
Given generalized coordinates q = (q1 , . . . , qn) ∈ E, a holonomic
constraint is a set of constraint equations:
hi(q) = 0, i = 1, . . . , kQ = h−1(0) is a manifold of dim n − k ≜ m if the constraints are
linearly independent.
TqQ ∶ {V ∈ TqE∣dhi ⋅V = 0,∀i = 1, . . . , k} ⊂ TqEsubspace of permissible velocities.
T∗qQ⊥ ∶ {f ∈ T∗q E∣⟨f , v⟩ = 0,∀V ∈ TqQ} = span{dh1 , . . . , dhk}subspace of constraint forces.
De�nition: Constraint forces
Γ = ∂h∂q
T⋅ λ
λ ∈ Rk is the vector of relative magnitudes of constraint forces.
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
44
De�nition: Pfa�an Constraints
A Pfa�an constraint has the form:
A(q)q = 0,A(q) ∈ Rk×n
Given a Pfa�an constraint,
∆q = {Vq ∈ TqE∣A(q) ⋅Vq = 0} ⊂ TqEDistribution of permissible velocities.
De�nition:
A(q)q = 0 is holonomic (or integrable) i� ∆q is an involutive
distribution, or i�
∃hi ∶ E↦ R, i = 1, . . . , k s.t. ∆q = TqQ,Q = h−1(0)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
45
§ Constraint forces:
Γ = AT(q) ⋅ λ, λ ∈ Rk
§ Kinetic energy:
T(q, q) = 1
2qT ⋅M(q) ⋅ q
§ Potential energy:
V(q)§ Lagrangian:
L(q, q) = T(q, q) −V(q)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
46
◻ Lagrange’s equations with constraints:
M(q)q + C(q, q) +N(q) +AT(q)λ = F◻ Explicit solution for constraint forces:
A(q)q + A(q)q = 0(AM−1AT)λ = AM−1(F − C −N) + Aqλ = (AM−1AT)−1(AM−1(F − C −N) + Aq)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
47
◇ Example:
x
y
(x, y)
mg
l
θ
x2 + y2 = l2
[x y]²A(q)
[ xy ] = 0
L(q, q) = 1
2m(x2 + y2) −mgy
[ m 00 m ] [ x
y ] + [ 0mg ] + [ xy ] λ = 0
λ = (AM−1AT)−1(AM−1(F − C −N) + Aq)= −
m
l2(gy + x2 + y2)
= ∥[ xy ] λ∥ = mg
ly +
m
l(x2 + y2)
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
48
◻ Lagrange-d’Alembert formulation:Given the Pfa�an constraint A(q)q = 0 and virtual displacement
δq ∈ Rk, we have:
�eorem 1 (D’alembert Principle):
Forces of constraints do no virtual work!(AT(q)λ) ⋅ δq = 0 for A(q)δq = 0
1717–1783
�eorem 2 (Lagrange-d’Alembert Equation):
( ddt
∂L
∂q−∂L
∂q− τ) ⋅ δq = 0,A(q)δq = 0
1736-1813
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
49
Let A(q) = [A1(q) A2(q)], and A2(q) ∈ Rk×k is invertible, then
δq1 ∈ Rn−k are free variables:
δq2 = −A−12 (q)A1(q)δq1
⇒ ( ddt
∂L
∂q−∂L
∂q− τ) ⋅ δq
= ( ddt
∂L
∂q1−
∂L
∂q1− τ1) ⋅ δq1 + ( d
dt
∂L
∂q2−
∂L
∂q2− τ2) ⋅ δq2
= ( ddt
∂L
∂q1−
∂L
∂q1− τ1) ⋅ δq1 + ( d
dt
∂L
∂q2−
∂L
∂q2− τ2) ⋅ (−A−12 A1)δq1
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
50
As δq1 ∈ Rn−k is free,
( ddt
∂L
∂q1−
∂L
∂q1− τ1) − AT
1 A−T2 ( ddt
∂L
∂q2−
∂L
∂q2− τ2) = 0
Lagrange-d’Alembert equation
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
51
◇ Example: Dynamics of a rolling disk
Pfa�an constraint:⎧⎪⎪⎨⎪⎪⎩x − ρ cos θ ϕ = 0
y − ρ sin θϕ = 0 (x , y)θ
ϕ
⇒ A(q)q = [ 1 0 0 −ρ cos θ0 1 0 −ρ sin θ ] q = 0
L(q, q) = 12m(x2 + y2) + 1
2I1θ
2 +12I2ϕ
2
Lagrange-d’Alembert equation:
⎛⎝⎡⎢⎢⎢⎢⎣m
mI1
I2
⎤⎥⎥⎥⎥⎦q −
⎡⎢⎢⎢⎢⎣00τθτϕ
⎤⎥⎥⎥⎥⎦⎞⎠ ⋅ δq = 0
where A(q) ⋅ δq = 0.
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
52
As
{ δx = ρ cos θ ⋅ δϕδy = ρ sin θ ⋅ δϕ
the equation of motion is:
([ 0 0mρcθ mρsθ ] [ x
y ] + [ I1 00 I2
] [ θϕ] − [ τθτϕ ]) ⋅ [ δθ
δϕ ] = 0⇒ [ 0 0
mρcθ mρsθ ] [ xy ] + [ I1 0
0 I2] [ θ
ϕ] = [ τθτϕ ]
As ⎧⎪⎪⎨⎪⎪⎩x = ρ cos θ ⋅ ϕ − ρ sin θ ⋅ θ ϕ
y = ρ sin θ ⋅ ϕ + ρ cos θ ⋅ θ ϕ
⇒ [ I1 00 I2 +mρ2 ] [ θ
ϕ] = [ τθτϕ ]
Solve for (θ(t), ϕ(t)), and then solve for (x(t), y(t)) from:⎧⎪⎪⎨⎪⎪⎩x = ρ cos θ ⋅ ϕ
y = ρ sin θ ⋅ ϕ⇐ 1st order di�erential equation
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
53
◻ Nature of nonholonomic constraints:
Consider q = (r, s) ∈ R2×R with Pfa�an constraint,
s + aT(r)r = 0, a(r) ∈ R2
Lagrangian
L = L(r, r, s)and constrained Lagrangian
Lc(r, r) = L(r, r,−aT(r)r)⇒ Lagrange’s equation:
d
dt
∂Lc∂ri−∂Lc∂ri= 0, i = 1, 2
⇒d
dt( ∂L∂ri− ai(r)∂L
∂s) − ⎛⎝
∂L
∂ri−∂L
∂s∑j
∂aj∂ri
rj⎞⎠ = 0
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
54
⇒ ( ddt
∂L
∂ri−∂L
∂ri) − ai(r)( d
dt
∂L
∂s−∂L
∂s) = ∂L
∂s
⎛⎝ai(r) −∑j
∂aj∂ri
rj⎞⎠´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶
≠ 0
(∗)
If the constraint is holonomic, i.e.
ai(r) = ∂h
∂rifor some h ∶ E↦ R
then RHS (right hand side) of (∗) equals
∂L
∂s
⎛⎝∑j
∂2h
∂ri∂rjrj −∑
j
∂2h
∂rj∂rirj⎞⎠ = 0
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
55
◻Metric, duality and orthogonality on TqE:
qx1⋯
xnTqQ
Q = h−1(0)K =
1
2qTM(q)q
=1
2≪ q, q≫M
TqQ⊥ = {V1 ∈ TqE∣≪ V1,V2 ≫M= V
T1 MV2 = 0,∀V2 ∈ TqQ}
T∗qQ⊥ = {f ∈ T∗q E∣⟨f ,V⟩ = 0,∀V ∈ TqQ} ∶ constraint forces
TqE = TqQ⊕ TqQ⊥,T∗q E = T∗qQ⊕ T∗qQ⊥
De�nition:Mb∶ TqE ↦ T∗q E, ⟨M♭V1 ,V2⟩ = VT
1 MV2 =≪ V1 ,V2 ≫M
M♯ ∶ T∗q E ↦ TqE,M♯ =M♭−1
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
56
Reciprocal
Property 4:
Under the basis ∂∂qi
and dqi, i = 1, . . . , n of TqE and T∗q Erespectively, the matrix representation ofM♭ andM♯ isM and
M−1 respectively.Property 5:
M♯(T∗qQ) = TqQ
M♯(T∗qQ⊥) = TqQ⊥
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
57
Givenh ∶E ↦ R
k,m = n − k
h∗ ≜ Tqh ∶TqE ↦ Th(q)Rk
h∗ ≜ T∗q h ∶T∗h(q)Rk ↦ T∗q Ewe haveProperty 6:
kerh∗ = TqQ, h∗(TqQ⊥) = Th(q)Rk, h∗(T∗h(q)Rk) = T∗qQ⊥
T∗q E T∗q Rk
TqE Th(q)Rk
h∗
M♯ M♯2
h∗
M♯2 = h∗ ○M♯ ○ h∗
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
58
Lemma 1: �e map (I − Pω) ∶ T∗q E ↦ T∗qQ⊥ given by
(I − Pω) = h∗ ○M♭2 ○ h∗ ○M♯is a well-de�ned projection map, with the property:(I − Pω)f1 = 0,∀f1 ∈ T∗qQ
(I − Pω)f2 = f2,∀f2 ∈ T∗qQ⊥Proof :
Given f1 ∈ T∗qQ,M
♯(f1) ∈ TqQ = kerh∗, then (I − Pω)(f1) = 0. Forf2 ∈ T
∗qQ⊥, ∃λ ∈ R
n−m s.t. f2 = h∗λ, and
(I − Pω)f2 = h∗M♭2h∗M♯h∗λ= h∗λ = f2
⇒ Pω ∶ T∗q E ↦ T∗qQ
is a well-de�ned projection map. Similarly,
PT ∶ TqE ↦ TqQ, PT = I −M♯h∗Mb
2h∗
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
59
and (I − PT) ∶ TqE ↦ TqQ⊥ are projection maps.
Lemma 2:PωM =MPT
Pωh∗ = h∗PT = 0
PT = PTω
For nonholonomic constraints:h∗ ← A(q)h∗ ← A∗(q)
TqQ← ∆q
T∗qQ⊥ ← span{ai(q), i = 1, . . . , k}application in hybrid velocity/force control.
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
60
◻ Lagrange’s equations of motion:
M(q)q + C(q, q) +N +AT(q)λ = τλ = (AM−1AT)−1(AM−1(τ −C −N) + Aq)
M(q)q + AT(AM−1AT)Aq + PωC + PωN = PωτPω = I −A
T(AM−1AT)−1AM−1C = PωC
N = PωN
τ = Pωτ
Mθ + AT(AM−1AT)−1Aθ = PωMθ ≜ Mθ ∶ intertia forces in T∗qQDe�nition: Dynamics in T∗qQ
Mθ + C + N = τ
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
61
(I − Pω)(Mq +C +N) = (I − Pω)τ +ATτLet
PT = I −M−1AT(AM−1AT)−1A
and
PωM =MPTthen the dynamics in T∗Q⊥:
M(I − PT)(q +M−1C) = (I − Pω)(τ −N) + ATλ
◻ Geometric Interpretation:∇↔M
Mq +C +N = τ +ATλ⇔M∇qq = τ −N + ATλ∇↔ induced metric on TqQ
S ∶TQ⊗ TQÐÐÐÐÐÐ→ N(Q) :2nd fundamental form
↑ ↑tangent vector �eld normal vector �eld
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
62
∇XY = ∇XY + S(X,Y)M (I − PT)(q +M−1C)´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶
S(q,q)= (I − Pω)(τ −N) + ATλ
MS(q, q): centrifugal force due to curvature of Q in E
M∇qq = τ − N
MS(q, q) = (I − Pω)(τ −N) + ATλ
⎫⎪⎪⎪⎬⎪⎪⎪⎭for hybrid control design
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
63
◇ Example: Dynamics of a Spherical Pendulum
K =1
2m(x2 + y2 + z2)
=1
2qTMq
q = (x, y, z)T ,M = mI
h ∶qTq − r2 = 0
A = (x, y, z),M♯2 = AM♯AT = r2/mPω =
1
r2
⎡⎢⎢⎢⎢⎣y2 + z2 −xy −xz−yz x2 + z2 −yz−zx −zy x2 + y2
⎤⎥⎥⎥⎥⎦I − Pω =
1
r2
⎡⎢⎢⎢⎢⎣x2 xy xzyx y2 yzzx zy z2
⎤⎥⎥⎥⎥⎦
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
64
PT = PTω = Pω(µ, ν): Spherical coordinates
q = (r cos µ cos ν, r cos µ sin ν, r sin µ)T∇qq = PT(∇qq)= [ −r sin µ cos ν −r sin ν−r sin µ sin ν r cos ν
r cos µ 0] [ v1
v2 ]S(q, q) = (I − PT)(∇qq)
= (−µ2 − cos2 µν2)[ r cos µ cos νr cos µ sin ν
r sin µ]
wherev1 = µ + sin µ cos µν2
v2 = cos µν − 2 sin µµν
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
65
◻ Control Algorithm:
1 holonomic constraints:q ∶ coordinates of Q
q = ψ(q)⇒ q = J ⋅ ˙q
τ =MJ(¨qd − Kv˙e − Kpe) + C1 +N +AT(−λd +KI ∫ (λ − λd))
2 nonholonomic constraints:
Let J(q) ∈ Rn×m be s.t. AJ = 0. Write q = J ⋅ u for some u
τ =MJ(ud −Kp(u − ud)) +MJu + C +N + AT(−λd + KI ∫ (λ − λd))
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
66
◇ Example: 6-DoF manipulator on a sphere withfrictionless point contact
Contact constraint:
vz = 0⇔ [0 0 1 0 0 0]Adg−1flf
Vof = 0
⇒Holonomic constraint:
η = (αTo , α
Tf ,ψ): Parametrization of Q
Pω = diag(1, 1, 0, 1, 1, 1)Newton-Euler Equations of motion:
MVof − adTVof
MVof = Fm +G +ATλ
Vof =
⎡⎢⎢⎢⎢⎢⎣RψMo −Mf 00 0 0
RψRoKoMo −RoKfMf 0−ToMo −TfMf 1
⎤⎥⎥⎥⎥⎥⎦⎡⎢⎢⎢⎣αoαfψ
⎤⎥⎥⎥⎦ ≜ JηMJη +C1 = Fm +G +A
Tλ ∗
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
67
Pω(∗) ∶Mη + C1 = B1Fm + B1G
−ϕ3 − λ = b2Fm + b2G
F = [f1 f2 f4 f5 f6]T = M(ηd −Kv˙e − Kpe) + C1 − B1G
f3 = −ϕ3 − λd + KI ∫ (λ − λd) − b2Gτ = JTs Fm
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
68
◇ Example: 6-DoF manipulator rolling on a sphere⎡⎢⎢⎢⎢⎣
1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 0 0 1
⎤⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶A1(q)
Vlolf = 0
fc = AT1 λ, λ ∈ R
4
[ ωxωy ] = −R0(Kf + RψKoRψ)Mf αf
Vof = Adgflf ⋅Vlo lf
= Adgflf
⎡⎢⎢⎢⎢⎢⎢⎣
000
−Ro(Kf + RψKoRψ)Mfo
⎤⎥⎥⎥⎥⎥⎥⎦αf
≜ Jf αfspan{Jf }: Not involutive
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
69
MJf αf + (MJf αf − adTJf αf
MJf αf ) = Fm +G + ATλ
Fm =MJf (αfd − Kp(αf − αfd)) + (MJf αf − adTJf αf
MJf αf )+ AT(−λd + ∫ (λ − λd)) −G
◇ Example: Redundant parallel manipulatorθ = (θ1 , . . . , θ6) ∈ Eθa = (θ1 , θ3, θ5)θp = (θ2, θ4, θ6)
H(θ) =⎡⎢⎢⎢⎢⎢⎣xa + lc1 + lc12 − xb − lc3 − lc34ya + ls1 + ls12 − yb − ls3 − ls34xa + lc1 + lc12 − xc − lc5 − lc56ya + ls1 + ls12 − yc − ls5 − ls56
⎤⎥⎥⎥⎥⎥⎦= 0
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
70
Mi(θ) ∈ R2×2: ith chain
M(θ) = diag(M1(θ), . . . ,M3(θ))M(θ)θ + C +N = τ + ATλIf all joints are actuated,
Position control of end-e�ector+
internal grasping forceAs τ2, τ4, τ6 = 0,
θ ∈ R2∶ local parametrization of Q = H−1(0)
θ = ψ(θ) ∶embedding of Q in E
θ = J ˙θ
Chapter 4RobotDynamics andControl
LagrangianEquations
InertialProperties ofRigid Body
Dynamics ofan Open-chainManipulator
Newton-EulerEquations
Coordinate-invariantalgorithms forrobotdynamics
Lagrange’sEquations withConstraints
4.6 Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control
71
Given Pω ∶ T∗θ E ↦ T∗θQ, the dynamics in T∗θQ is given by:
PωMJ ¨θ + Pω(C1 +N) = Pωττ = (τ1 , τ3, τ5)
Pω = (P1 , P3, P5)τ = Pω τ = Pωτ ∈ R
6
τ = PωMJ( ¨θd −Kv˙e −Kpe) + Pω(C1 +N)