Dynamics of robotic multibody systems

4820 Robotic Multibody Systems

Prof. Marcello Romano

Dr. Josep Virgili-Llop

Naval Postgraduate School

Spacecraft Robotics Laboratory

October 30, 2017

c Prof. Marcello Romano & Dr. Josep Virgili-Llop. Please do not redistribute without authors’ consent.

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 1 / 25

Overview

1 Introduction

2 Accelerations

3 Equations of Motion

4 Generalized Equations of Motion

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 2 / 25

 Introduction

Summary of Unit 1 and 2

So far we learnt how to solve at any instant:

The Direct Kinematics and Inverse Kinematics of a MBS: i.e.
how to express and use the direct and inverse relationship between
the values of the generalized coordinates of the system and the pose
of each link (i.e. the orientation and position of the associated CCS
and its origin).
The Direct Differential Kinematics and Inverse Diff. Kinematics
of a MBS: i.e. how to express and use the direct and inverse
relationship between the values of the generalized coordinates of the
system and the twist of each link (i.e. the angular velocity and linear
velocity of the associated CCS and its origin).

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 3 / 25

 Introduction

Learning goal of this Unit

In this unit we will study the direct dynamics problem of a MBS, i.e.
how to express mathematically and use the relationship between all of the
forces and torques acting on the MBS and the accelerations of each link
(i.e. the angular accelerations and linear accelerations of the associated
CCS and its origin).

The actions (forces and torques) acting on a MBS can be

External actions e.g. due to the interaction of the End Effector with
the environment or the use of base vehicle actuators exerting external
actions on the base or to impacts of any parts of the MBS with
another object.
Internal actions e.g the torque or forces exerted by the motors at the
joints or the use of base vehicle actuators exerting internal actions
such as Spacecraft Angular Momentum Exchange Devices.

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 4 / 25

 Accelerations

Propagation of Angular Velocities (repeated)

Consider any two consecutive links of a branch: the absolute angular
velocity of link/CCS Li+1 is recursively obtained by the chain rule valid for
angular velocities
ω
~ i+1 = ω
~ i + θ̇i+1 êi+1 ∀i = 0, ..., n − 1 (1)

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 5 / 25

 Accelerations

Propagation of the angular accelerations

The absolute angular acceleration of link/CCS Li+1 is obtained by taking
the time derivative w.r.t. the inertial frame of its angular velocity (Eq. 1)
i.e. (REM: the dot over a vector indicates time derivative w.r.t. the
inertial frame, while dot over a scalar indicates time derivative)
d~
ωi+1 d θ̇i+1 êi+1
~˙ i+1 ,
ω ~˙ i +
=ω (2)
dt I dt
I

d θ̇i+1 êi+1 d θ̇i+1 d êi+1 d êi+1

= êi+1 + θ̇i+1 = θ̈i+1 êi+1 + θ̇i+1
dt dt dt I dt I
I I
(3)
d êi+1 d êi+1
= ~ i × êi+1 = ω
+ω ~ i × êi+1 (4)
dt I dt Li
Combining these expressions:
~˙ i+1 = ω
ω ~˙ i + θ̈i+1 êi+1 + θ̇i+1 (~
ωi × êi+1 ) (5)
Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 6 / 25
 Accelerations

Propagation of Linear Velocities (repeated)

It is (see figure)
~ri+1 = ~ri + ~bi + ~gi+1 ∀i = 0, ..., n − 1, (6)
which, by differentiating both sides with respect to time w.r.t. the inertial
frame gives the absolute linear velocity of the origin of the CCS Li+1 , as
follows
~r˙i+1 = ~r˙i + ~ci+1,i × ω
~ i + d˙i+1 êi+1 + θ̇i+1 (êi+1 × ~gi+1 ) (7)

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 7 / 25

 Accelerations

Propagation of the linear accelerations

The absolute linear acceleration of the origin of link/CCS Li+1 is

recursively obtained by taking the time derivative w.r.t. the inertial frame
of the linear velocity of that point expressed as in Eq. 7, i.e.

d~r˙i+1 d~r˙i d~ci+1,i × ω

~i
~¨ri+1 , = + +
dt dt dt I
I I
d d˙i+1 êi+1 d θ̇i+1 (êi+1 × ~gi+1 )
+ (8)
dt dt
I I

Let’s go through each term. The first one:

d~r˙i
, ~¨ri (9)
dt
I

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 8 / 25

 Accelerations

Second term:
d~ci+1,i × ω
~i
= ~c˙ i+1,i × ω ~˙ i
~ i + ~ci+1,i × ω (10)
dt I

Third term:

d d˙i+1 êi+1
= d¨i+1 êi+1 + d˙i+1 (~
ωi × êi+1 ) (11)
dt
I

where Eq. 4 has been used.

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 9 / 25

 Accelerations

Fourth term:

d θ̇i+1 (êi+1 × ~gi+1 ) d (êi+1 × ~gi+1 )

= θ̈i+1 (êi+1 × ~gi+1 ) + θ̇i+1 (12)
dt dt I
I

where
d (êi+1 × ~gi+1 ) d (êi+1 × ~gi+1 )
= ~ i+1 × (êi+1 × ~gi+1 )
+ω (13)
dt I dt Li+1

but
d (êi+1 × ~gi+1 ) d êi+1 d~gi+1
= × ~gi+1 + êi+1 × = 0 (14)
dt Li+1 dt Li+1 dt Li+1

as vectors êi+1 is fixed w.r.t. link Li+1 and ~g˙ i+1 = d˙i+1 êi+1 . Therefore

d θ̇i+1 (êi+1 × ~gi+1 )

= θ̈i+1 (êi+1 × ~gi+1 ) + θ̇i+1 [~
ωi+1 × (êi+1 × ~gi+1 )]
dt
I
(15)
Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 10 / 25
 Accelerations

By substituting all of the terms back into Eq. 8, it yields

~¨ri+1 = ~¨ri + ~c˙ i+1,i × ω ~˙ i + d¨i+1 êi+1 + d˙i+1 (~

~ i + ~ci+1,i × ω ωi × êi+1 ) +
θ̈i+1 (êi+1 × ~gi+1 ) + θ̇i+1 [~
ωi+1 × (êi+1 × ~gi+1 )] (16)

yet, noting that

~ i × êi+1 = ω
ω ~ i+1 × êi+1 (17)
as  
~ i+1 × êi+1 = ω
ω ~ i + θ̇i+1 êi+1 × êi+1 = ω
~ i × êi+1 (18)

permits to write the linear velocity expression as follows:

~¨ri+1 = ~¨ri + ~c˙ i+1,i × ω ~˙ i + d¨i+1 êi+1 + d˙i+1 (~

~ i + ~ci+1,i × ω ωi+1 × êi+1 ) +
θ̈i+1 (êi+1 × ~gi+1 ) + θ̇i+1 [~
ωi+1 × (êi+1 × ~gi+1 )] (19)

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 11 / 25

 Accelerations

Screw algebra formulation: twist matrix

(repeated)

When the angular and linear velocity vectors of link/CCS Li+1 are
projected onto the inertially fixed CCS I, their scalar components are
collected in the column matrices ωi+1I I .
and r˙i+1
By using the screw theory notation, these velocity component matrices can
be encapsulated in a 6 × 1 (absolute) twist matrix ti+1 ∈ R6 (which is
expressing, notionally speaking, the absolute ’time-derivative’ of the pose
of link/CCS Li+1 ).  I 
ω
ti+1 , Ii+1 (20)
r˙i+1

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 12 / 25

 Accelerations

twist-rate

The accelerations of each links/CCS Li+1 of the MBS can be expressed by

using the twist-rate so defined
 I 
ω̇
ṫi+1 , Ii+1 (21)
r¨i+1

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 13 / 25

 Accelerations

Accelerations Equations in Scalar Form

By projecting Eq. 5 and 19 onto CCS I, ∀i = 0, ..., n − 1 of a specific
branch, it yields
I
h
× I i
ω̇i+1 = ω̇iI + θ̈i+1 êi+1
I
+ θ̇i+1 ωiI êi+1 (22)

I

×
×
r¨i+1 = r¨iI + ci+1,i
I I
ω̇iI + ċi+1,i ωiI +
I I
h
× I i
d¨i+1 êi+1 + d˙i+1 ωi+1 êi+1 +
h
I

× I i h
I

× I
× I i
θ̈i+1 êi+1 gi+1 + θ̇i+1 ωi+1 êi+1 gi+1 (23)

while we found in Unit 2

I
ωi+1 = ωiI + θ̇i+1 êi+1
I
(24)
× I × I
h i
I
= r˙iI + ci+1,i
I I
ωi + d˙i+1 êi+1 I



r˙i+1 + θ̇i+1 êi+1 gi+1 , (25)

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 14 / 25

 Accelerations

DDK in terms of Twist (repeated)

Given two successive links/CCS Li & Li+1 on the same branch of a MBS
the DDK Eq. is
ti+1 = Bi+1,i ti + pi+1 q̇i+1 (26)
with twist, twist-propagation matrix and twist-propagation ’vector’ as
 I 
ω
ti+1 , Ii+1 (27)
r˙i+1
" #
I3 03×3
Bij ,  × (28)
cijI I3
" #
ê I
i+1

if joint Ji+1 is revolute ↔ q̇i+1 = θ̇i+1

 ê I
× g I


i+1# i+1
pi+1 , " (29)
 03×1 ˙
if joint Ji+1 is prismatic ↔ q̇i+1 = di+1


 ê I

i+1

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 15 / 25

 Accelerations

Furthermore it is (repeated)

 
ω0
t0 = P0 (30)
r˙0
where 


I6 if ω0 ≡ ω0I


P0 , " # (31)
R 0 03×3
ω0L0

if ω0 ≡



 0
3×3 I3
with
R 0 ≡ I R L0 (32)

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 16 / 25

 Accelerations

Accelerations in terms of Twist Rate

By using Eq. 22 and 23 and the following definitions

" #
03×3 03×3
Ḃij ,  I × (33)
ċij 03,3
"
× #
ωiI 03×3
Ωi ,
× (34)
03×3 ωiI
Given two successive links/CCS Li & Li+1 on the same branch of a MBS,
the acceleration of Li+1 can be written recursively as follows

ṫi+1 = Bi+1,i ṫi + Ḃi+1,i ti + Ωi+1 pi+1 q̇i+1 + pi+1 q̈i+1 (35)

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 17 / 25

 Accelerations

Furthemore, it is    
ω0 ω̇
ṫ0 = Ṗ0 + P0 0 (36)
r˙0 r¨0
with 
06×6
" for ω0 = ω0I

× #
Ṗ0 = ωiI R0 03×3 (37)
 for ω0 = ω0L0
03×3 03×3


Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 18 / 25

 Equations of Motion

Resultant torque, force and wrench

acting on the ith link

Let us call ni ∈ R3 the column matrix of projection on the inertial CCS of

the resultant external torque vector on the ith link, considering as pole the
CoM of the link.
Let us call fi ∈ R3 the column matrix of projection on the inertial CCS of
the resultant external force vector on the ith link, considering it applied to
the CoM of the linik.
By using the screw algebra notation these two actions can be encapsulated
into a six-dimensional wrench column matrix wi ∈ R6 .
 
n
wi = i (38)
fi

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 19 / 25

 Equations of Motion

Equations of motion of a single link

The dynamic equations (aka Newton-Euler Equations) of the ith link
–considered as a rigid body– can be written as
ni = Ji ω̇i + ωi× Ji ωi (39a)
fi = mi r¨i (39b)
With J denoting the 3-by-3 inertia matrix of component of the inertia
tensor wrt the CoM onto the inertial CCS.

If the following 6 × 6 mass matrix Mi and its derivative are defined

   × 
Ji 03×3 ωi Ji 03×3
Mi , Ṁi = (40)
03×3 mi I3×3 03×3 03×3
the Newton-Euler’s equations of motion can be written in the following
compact form
Mi ṫi + Ṁi ti = wi (41)
Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 20 / 25
 Generalized Equations of Motion

Eq. of Motion of the Whole MBS

in generalized twist, twist rate and wrench
The compact equation of motion can then be generalized for the whole
MBS as follows
M ṫ + Ṁt = w (42)
with w being the 6 (n + 1) × 1 generalized wrench column matrix and M
the 6 (n + 1) × 6 (n + 1) generalized mass matrix.
 
w0
w1 
w = .  (43)
 
 .. 
wn
 
M0
 M1 
M = (44)
 
.. 
 . 
Mn
Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 21 / 25
 Generalized Equations of Motion

The wrenches can be decomposed in non-contributing wrenches w n and

contributing wrenches w c . The non-contributing wrenches do not
contribute work into the system, as these wrenches are perpendicular to
the link velocity. The contributing wrenches can be further classified by
the ones provided by the joint actuators w q (control wrenches) and the
wrenches due to external or internal interactions w F .

w = wc + wn = wn + wq + wF (45)

The joint actuator wrenches w q provided by the actuators on the

q
manipulator joints wm are internal wrenches with respect to multibody
q
system. Therefore, wm only cause internal system re-configuration keeping
a constant linear and angular momentum. In contrast, the wrenches
provided by the base-link w0q can be be external (e.g. due to thrusters) or
internal (e.g/ due to MEDs) to the multibody system and produce motion
of the entire system, altering the system’s linear and angular momentum.
The other non-control external or internal wrenches w F can be external
with respect to the system, as contact forces (e.g. end-effector loads), or
internal, as friction at the joints.
Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 22 / 25
 Generalized Equations of Motion

Velocity Transformation Matrix (repeated)

It is useful to define a velocity transformation matrix (aka natural
orthogonal complement) N ∈ R6(n+1)×(6+n) such that

t = Nu, (46)

which maps the ’joint space’ velocities (i.e. the velocity of the base and
joints’ speeds) –collected in the generalized velocity matrix u– to the
velocities in the operational space –collected in the generalized twist
matrix t.
With  
ω0
 r˙0 
 
u =  q̇1  (47)
 
 .. 
 . 
q̇n

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 23 / 25

 Generalized Equations of Motion

As the non-contributing wrenches w n do not perform any work, they can

be eliminated by premultiplying the generalized equation of motion Eq. 42
by N T  
N T M ṫ + Ṁt = N T w q + N T w F (48)

where it has been taken into account that t T w n = u T N T w n = 0.

Applying the twist t and the twist-rate ṫ vector into Eq. 48 provides the
generalized equation of motion:

H u̇ + Cu = τ (49)

H = N T MN (50)
 
C = N T M Ṅ + ṀN (51)

τ = τq + τF (52)
τ F = NT w F (53)
τ q = NT w q (54)
Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 24 / 25
 Generalized Equations of Motion

The (n + 6) × (n + 6) generalized inertia matrix H is a positive definite

symmetric matrix and can be subdivided in the 6 × 6 base-link inertia
matrix H0 , the n × n manipulator inertia matrix Hm and the 6 × n
base-manipulator coupling inertia matrix H0m . Similarly the generalized
convective inertia matrix, which is not symmetric, can also be subdivided
in analogous subparts.  
H0 H0m
H= T (55)
H0m Hm
 
C0 C0m
C= (56)
Cm0 Cm

Prof. Romano & Dr. Virgili-Llop Dynamics October 30, 2017 25 / 25