Robot Dynamics III

Chew Chee Meng

1
What you would learn

• Recursive computation of inverse

dynamics
• Moving coordinate frame
• Luh‐Walker‐Paul’s algorithm

2
Introduction
• Inverse dynamics:
• To find appropriate inputs (joint torques) to produce
desired outputs (joint displacement)
• Important for robot control/programming

Arm linkage
dynamics
𝜏 𝑞
: :
𝜏 𝑞
𝑞
Inverse :
𝑡 dynamics 𝑞 𝑡

3
Introduction
• For example, if we want to track a circular path very fast
where centrifugal and coriolis forces are large, we want to
introduce a feedforward model to the control system:

Dynamic
model

𝐪𝐫 𝛕 Robot 𝐪
Arm

4
Introduction
• Outputs (joint torques) to be applied at each instant are
obtained by evaluating the RHS of the closed‐form dynamic
equations:

𝜏 𝐻 𝑞 ℎ 𝑞 𝑞 𝐺 𝑖 1, … , 𝑛

𝜏 𝐻 𝑚 ,𝐼 𝐻 , ℎ and 𝐺 are
: : : configuration dependent (and
𝜏 :
𝐻 involved a lot of computation)
: :
𝜏 :
𝐻 𝑚 ,𝐼

𝑛 𝑛 𝑛

No of multiplications: 𝑂 𝑛
5
Introduction

ℎ 𝑞 𝑞

𝑂 𝑛

For n = 6, requires 66,271 multiplications for each

data point (Hollerbach 1980)

• Would like to reduce 𝑂 𝑛 to 𝑂 𝑛 for inverse dynamics (especially in

real time control)  That is, required number of operations varies
linearly with the no. of DOF.

6
Recursive Computation
• Key concept: Formulate dynamic equations in recursive form, e.g. based
on Newton‐Euler formulation  Link‐to‐link computation
• First phase: determine all kinematic variables needed for Newton‐
Euler formulation
• Second phase: Evaluate Newton‐Euler equations with computed
kinematic variable to determine joint torques

𝒗
𝝎
𝑞 ,𝑞 ,𝑞
𝒗
𝝎 Kinematics

𝑞 ,𝑞 ,𝑞
Dynamics 𝒇 , (specified)
𝑵 , (specified)
Figure: An open‐kinematic chain 7
 Recursive Computation
• First phase:
• Starts with first link
• Given joint displacement 𝑞 , joint velocity 𝑞 and joint acceleration
𝑞
• Determine linear and angular velocities and accelerations of
centroid C1 :𝒗 , 𝝎 , 𝒗 , 𝝎
• …
• Until all linear and angular velocities and accelerations of all links
are determined. 𝒗
𝝎
𝑞 ,𝑞 ,𝑞
𝒗 𝐶

𝝎 Kinematics
𝐶
𝑞 ,𝑞 ,𝑞
Dynamics 𝒇 , (specified)
𝑵 , (specified)
Figure: An open‐kinematic chain 8
 𝐛

Recursive Computation
• Second phase:
• Starts with last link n (Force/momentum relationship)
𝐟 , 𝐟 , 𝑚 𝐠 𝑚 𝒂 (66)

=> Coupling force between links n‐1 and n, 𝐟 , , can be determined.

• Similarly, coupling moments 𝐍 , can be determined using
moment/angular momentum relationship
• Repeat for link n‐1 and eventually to first link
Summary: recursive procedure for link i :
𝐟 , 𝐟, 𝑚 𝒂 𝐠 (67)
𝐍 , 𝐍, 𝐫 , 𝐟 , 𝐫, 𝐟, 𝐈𝛚 𝛚 𝐈𝛚 (68)

=> Joint torque can be computed depending on joint type:

For prismatic joint, 𝜏 𝐛 𝐟 ,
For revolute joint, 𝜏 𝐛 𝐍 ,
10
Moving Coordinate Frame
•𝑂 𝑥 𝑦 𝑧 : coordinate frame fixed to base (ground)
• 𝑂 𝑥𝑦𝑧 : rotating with angular velocity 𝝎, origin is
stationary
• Arbitrary vector 𝐬 fixed to 𝑂 𝑥𝑦𝑧
• Time rate of change of vector s as viewed from fixed frame:

𝑑𝐬
𝛚 𝐬 (71) 𝝎
𝑑𝑡 𝑧

𝑧 𝒔

𝑂
𝑦
𝑥
𝑂

𝑥 𝑦 11
Moving Coordinate Frame
• To find velocity and acceleration of link i+1, given motion of
link i and motion of link i+1 relative to link i  Analyze
relative motions defined in a moving coordinate frame.
•𝑂 𝑥 𝑦 𝑧 : coordinate frame fixed to base (ground)
• 𝑂 𝑥 𝑦 𝑧 : coordinate frame fixed to link i
•𝑂 𝑥 𝑦 𝑧 : coordinate frame fixed to link i+1

𝐛 𝑞
𝝎

12
 Moving Coordinate Frame
• From figure:
𝐫 , 𝐫 , 𝐫, (72)

Time rate of change of each term when viewed from base coordinate frame (fixed):
𝐫 , 𝐫 , 𝐫,
+ (73)

𝐯 𝐯

𝐫, represents relative displacement of link i+1 wrt moving coordinate frame i

13
Moving Coordinate Frame
Let n, t, b be unit vectors along coordinate axes of moving frame 𝑂 𝑥𝑦𝑧
Let x, y, z be the components of 𝐫 , wrt moving frame i: i.e. 𝐫 , 𝑥𝒏 𝑦𝒕 𝑧𝒃

𝑑𝐫 , 𝑑 𝑥𝒏 𝑦𝒕 𝑧𝒃
𝑑𝑡 𝑑𝑡
𝑑𝑥 𝑑𝑦 𝑑𝑧 𝑑𝒏 𝑑𝒕 𝑑𝒃
𝒏 𝒕 𝒃 𝑥 𝑦 𝑧 (74)
𝑑𝑡 𝑑𝑡 𝑑𝑡 𝑑𝑡 𝑑𝑡 𝑑𝑡

𝑑𝐫 , Velocity contribution from rotation of moving frame

𝑑𝑡 𝑑𝒏 𝑑𝒕 𝑑𝒃
𝑥 𝑦 𝑧 𝝎 𝒓, (from (71))
𝑑𝑡 𝑑𝑡 𝑑𝑡

Hence, equation (73) can be expressed as follows:

𝑑𝐫 ,
𝐯 𝐯 𝛚 𝐫, (76)
𝑑𝑡

14
Moving Coordinate Frame
In general, time rate of change of arbitrary vector moving relative to a rotating
coordinate frame is computed with differential operator:
𝑑 𝑑
𝛚 (77)
𝑑𝑡 𝑑𝑡

𝝎
𝑧

𝑧 𝒔

𝑂
𝑦
𝑥
𝑂

𝑥 𝑦

15
Moving Coordinate Frame
Second derivative of 𝐫 , (by applying time derivative on (76)
using differential operator (77)):

𝐯 𝐯 𝐫, 𝛚 𝐫,
𝐫, 𝛚 (78)

𝐚 𝐚

𝑑 𝑑𝐫 , 𝑑𝛚 𝑑𝐫 ,
𝐚 𝐚 𝐫, 𝛚 𝛚 𝐫,
𝑑𝑡 𝑑𝑡 𝑑𝑡 𝑑𝑡

𝑑 𝐫, 𝑑𝐫 ,
𝐚 𝐚 𝛚 𝐫, 2𝛚 𝛚 𝛚 𝐫, (79)
𝑑𝑡 𝑑𝑡

16
Luh‐Walker‐Paul’s Algorithm (1980)
• Recursive computation algorithm of Newton‐Euler
dynamic equation based on moving coordinate
frame
• First phase: Kinematic computations
• Angular velocity and acceleration of link i+1
• When joint i+1 is prismatic:
(80)

(81)

17
Luh‐Walker‐Paul’s Algorithm (1980)
• First phase: (cont.)
• Angular velocity and acceleration of link i+1
• When joint i+1 is revolute (frame i+1 rotates with angular
velocity 𝑞 𝐛 and angular acceleration 𝑞 𝐛 about zi
axis of moving coordinate frame attached to link i)
• Angular velocity of link i+1 referred to base frame:
𝛚 𝛚 𝑞 𝐛 (82)
• Angular acceleration of link i+1 obtained by taking time
derivative of (82):
𝑑
𝛚 𝛚 𝑞 𝐛 𝛚 𝑞 𝐛
𝑑𝑡

𝛚 𝛚 𝑞 𝐛 𝛚 𝑞 𝐛 (83)

18
Luh‐Walker‐Paul’s Algorithm
• Recursive equations for linear velocities and accelerations
(from Eqs (76) and (79))
• If joint i+1 is prismatic:
𝑑𝐫 , 𝑑𝟐 𝐫 ,
𝑞 𝐛 (84) 𝑞 𝐛 (85)
𝑑𝑡 𝑑𝑡

• Sub (84) and (85) into (76) and (79)):

𝐯 𝐯 𝑞 𝐛 𝛚 𝐫, (86)

𝐚 𝐚 𝑞 𝐛 𝛚 𝐫, 2𝛚 𝑞 𝐛 𝛚 𝛚 𝐫,
(87)

19
Luh‐Walker‐Paul’s Algorithm
• If joint i+1 is revolute:
𝑑𝐫 ,
𝑞 𝐛 𝐫, (88)
𝑑𝑡
𝑑 𝐫, 𝑑 𝑑𝐫 , 𝑑
𝑞 𝐛 𝐫,
𝑑𝑡 𝑑𝑡 𝑑𝑡 𝑑𝑡
𝑞 𝐛 𝐫, 𝑞 𝐛 𝑞 𝐛 𝐫, ) (89)

• Recursive equations for linear velocities (Sub (82) and

(88) into (76) ):
𝐯 𝐯 𝑞 𝐛 𝐫, 𝛚 𝐫,
𝐯 𝑞 𝐛 𝛚 𝐫,
𝐯 𝐯 𝛚 𝐫, (90)

20
Luh‐Walker‐Paul’s Algorithm
• If joint i+1 is revolute:
• Recursive equations for linear accelerations (Sub (83)
and (89) into (79) , and using identity of vector triple
products, i.e. 𝐚 𝐛 𝐜 𝐚 𝐜 𝐛 𝐛 𝐜 𝐚 and 𝐚
𝐛 𝐜 𝐚 𝐜 𝐛 𝐚 𝐛 𝐜:

𝑑 𝐫, 𝑑𝐫 ,
𝐚 𝐚 𝛚 𝐫, 2𝛚 𝛚 𝛚 𝐫,
𝑑𝑡 𝑑𝑡

𝐚 𝐚 𝛚 𝐫, 𝛚 𝛚 𝐫, (91)

21
Luh‐Walker‐Paul’s Algorithm
𝝎
• Newton‐Euler equations expressed in terms
of centroidal accelerations
• Let 𝐯 be velocity of origin of link i’s 𝒗
coordinate frame; and 𝛚 be angular 𝒓,
𝑂
velocity of link i 𝑐
• Centroidal velocity of link i: 𝒗
𝐯 𝐯 𝛚 𝐫, (92)

• Centroidal acceleration of link i (by applying

time derivative to both sides of (92), and
applying differential operator (77)):
𝑑 𝑑 𝑑
𝐯 𝐯 𝛚 𝐫,
𝑑𝑡 𝑑𝑡 𝑑𝑡

𝒂 𝒂 𝛚 𝐫, 𝛚 𝛚 𝐫, (93)
22
Luh‐Walker‐Paul’s Algorithm
• Inertia tensor 𝐈 varies with orientation of link i in Euler’s
equation (68) (if it is expressed in a frame having same
orientation as base frame) :
𝐍 , 𝐍, 𝐫 , 𝐟 , 𝐫 , 𝐟, 𝐈𝛚 𝛚 𝐈𝛚 (68)

• Let 𝐑 be 3x3 rotation matrix of frame i relative to frame 0

and 𝐈̅ be the inertia tensor expressed in coordinate frame
fixed to link i.
𝐈 𝐑 𝐈̅ 𝐑 (94)
Note:
𝐈̅ is invariant

23
Luh‐Walker‐Paul’s Algorithm
• In LWP algorithm, all variables/parameters are
expressed in link coordinates. Hence, no need to
compute (94)Eliminate the additional
computation
• If a variable referred to frame i is involved in
equation referred to frame i+1, premultiplied it by
(i.e. to express in frame i+1 before
substituting)
• In link coordinates, and , are constant.
• If joint i is revolute joint, , is constant too.

24
 τ 𝐛 .𝐟 , (*) (prismatic)
Summary of Luh‐Walker‐Paul’s Algorithm τ 𝐛 .𝐍 , (**) (revolute)
𝐯 ,𝛚 ,𝐚 𝐠, 𝛚 , (*) or
𝐟 , ,𝐍 , τ
𝑞 ,𝑞 ,𝑞 (**)

(80, 81, 86, 87)

or (67, 68)
(82, 83, 90, 91)
𝐯 ,𝛚 ,
𝐯 ,𝛚 ,𝐚 ,𝛚 , (92, 93) 𝐚 ,𝛚 ,
𝐟 , ,𝐍 ,
𝑞 ,𝑞 ,𝑞
(80, 81, 86, 87)
or
(82, 83, 90, 91) 𝐯 ,𝛚 ,
𝐚 ,𝛚 ,
𝐯 ,𝛚 ,𝐚 ,𝛚 , (92, 93)

𝐟 ,𝐍 (*) or τ
𝐯 ,𝛚 ,𝐚 ,𝛚 , , ,
(**)
𝑞 ,𝑞 ,𝑞
(80, 81, 86, 87) (67, 68)
or 𝐯 ,𝛚 ,
(82, 83, 90, 91) 𝐚 ,𝛚 ,
𝐟 , ,𝐍 ,
𝐯 ,𝛚 ,𝐚 ,𝛚 , (92, 93) 25
 Luh‐Walker‐Paul’s Algorithm

• Left half: kinematics computation (downwards)

• Right half: dynamics computation (upwards)
• Input: joint motions
• Output: joint torques
• Number of multiplications is 852 for general 6 dof
manipulator arm.

• Note:
• Can deal with case when base frame is in motion
(assume acceleration of base frame is known)
• Acceleration of gravity represented as part of base
frame => effect of gravity can be included without
extra computation (i.e. can exclude g in (67) and
(68)).

26
What have you learned

• Recursive computation of inverse dynamics

• Moving coordinate frame
• Luh‐Walker‐Paul’s algorithm

27