Marcelo H. Ang Jr.

1
CHAPTER 4
Robot Dynamics
Marcelo H. Ang Jr.
2
Dynamics
Joint Forces/Torque Joint Motion
(End Effector Motion)
LAGRANGIAN MECHANICS APPROACH
Energy-based approach to dynamics
L = K P
(q, q)

Lagrangian
Kinetic Energy
Potential Energy
The Lagrangian & energy terms can be expressed in
any convenient coordinate system (that will simplify
the problem)
Marcelo H. Ang Jr.
3
Dynamics
The dynamics equations, in terms of the coordinates
used to express the Kinetic & Potential Energy, are:
Ref: B.J. Torby, Advanced
Dynamics for Engineers,
Halt Rinehart, 1984.
i i
i
q
L
q
L
F

dt
d
=

where q
i
are the coordinates in which the kinetic &
potential energy are expressed.
q
i
= velocity
F
i
= corresponding Force torque in direction of q
i

generalized coordinates
closed form solution

Marcelo H. Ang Jr.
4
Dynamics
Ex: 1 - DOF Robot


L
m (pt mass)
T
K = mv
2
= m(Lu)
2
= mL
2
u
2
P = mg Lsinu
L = K P


L = mL
2
u
2
mg Lsinu

= mL
2
(2u) = mL
2
u

= -mg Lcosu

Marcelo H. Ang Jr.
5
Dynamics

L
T

dt
d
=

T = Torque (in direction of u)
T = mL
2
u + mg Lcosu

Note: This can be derived from Fundamental laws of
motion
e.g. Newtons eq. F = ma
T = Io
leading to Newton-Euler formulation
- recursive solution
Marcelo H. Ang Jr.
6
Kinetic Energy
Velocity of a Point on the manipulator:
i
r

{i}
T
i
{o}
Base
given a point
i
r described
with respect to link i, its
position in base coords is
r = T
i
i
r

Note that
i
r is constant
r = velocity of point

r )

T
(
dt
dr

i
i
1 j
i
j
j
q
q
r

=
= =

Marcelo H. Ang Jr.
7
Kinetic Energy
|| r ||
2
2
= r r = Trace (r r
T
)


= =
=
i
1 j
i
1 k
T i
k
i
i
i
] ) r
q
T
( r

T
[ Trace
k j
j
q q
q


= =
=
i
1 j
i
1 k
T
i
T i i
i
] )

T
( r r

T
[ Trace
k j
k j
q q
q q

Marcelo H. Ang Jr.
8
Kinetic Energy
Kinetic Energy of a particle of mass dm located on link i
at
i
r is
] )
q
T
( ) r dm r (

T
[ Trace
2
1

dm || ||
2
1
dk
T
k
i
i
1 j
i
1 k
T i i
i
2
2 i
k j
j
q q
q
r

= =
=
=


Kinetic Energy of link i
] )

T
( ) dm r r (

T
[ Trace
2
1
dk k
T
i
i
1 j
i link
T i i
i
1 k
i
i link
i i k j
k j
q q
q q

}
= =
= =

J
i
= pseudo inertia matrix
Marcelo H. Ang Jr.
9
Kinetic Energy
(
(
(
(
(
(

= =
} } } }
} } } }
} } } }
} } } }
}
dm dm i dm i dm i
dm i dm i dm i i dm i i
dm i dm i i dm i dm i i
dm i dm i i dm i i dm i
dm r r
z y x
z
z
z y z x
y z y
y
y x
x z x y x
x
T i i
2
2
2
i
J
where all integrals are
link i

and r
T
= [ i
x
i
y
i
z
1 ]
T
Symmetric matrix
Marcelo H. Ang Jr.
10
Kinetic Energy
2
I I I -
dm x
zz yy xx
2
+ +
=
}
2
I I I
dm y
zz yy xx
2
+
=
}
2
I I I
dm z
zz yy xx
2
+
=
}
Recall That
I
xx
= (y
2
+ z
2
) dm I
yy
= (x
2
+ z
2
) dm I
zz
= (x
2
+ y
2
) dm

I
xy
= xy dm I
xz
= xz dm I
yz
= yz dm

mx = x dm my = y dm mz = z dm
Marcelo H. Ang Jr.
11
Kinetic Energy
Then
(
(
(
(
(
(
(
(
(

+
+
+ +
=
i i i i i i i
i i
izz iyy ixx
iyz ixz
i i iyz
izz iyy ixx
ixy
i i ixz ixy
izz iyy ixx
m z m y m x m
z m
2
I - I I
I I
y m I
2
I I - I
I
x m I I
2
I I I -

i
J
Marcelo H. Ang Jr.
12
Kinetic Energy
The total kinetic energy of an N-joint manipulator
is then

= = = =
= =
N
1 i
T
i
i
1 j
i
1 k
i
N
1 i
i
] )

T
(

T
[ Trace
2
1
k k
k j
k
i
j
q q
q
J
q

k
T
i
T
k
i
q
T
)
q
T
( that Note
c
c
=
c
c
neglecting
actuator
inertia
Adding effect of actuators
K
actuator
i
= I
a
i
q
i
2
I
a
i
= actuator inertia, rotational joint i
= equivalent mass, translational joint i

Marcelo H. Ang Jr.
13
Potential Energy
P = -mg r because g is opposite height
above some reference zero
r = position of center of mass of object

g = gravity force vector
Marcelo H. Ang Jr.
14
Potential Energy
Ex: a mass m at r = 10i + 20j + 30k in a gravity field
g = 0i + 0j + -32.2k has a P = 966 m
The potential energy of a link whose center of mass is
described by
i
r
i
with respect to link i coord frame T
i
is then
P
i
= -m
i
g
T
T
i

i
r
i
reference is base frame
where g = [ g
x
g
y
g
z
0 ]
T
in base frame coords
Total Potential Energy is

=
=
N
1 i
i
i
i i
r T m - P
T
g
Marcelo H. Ang Jr.
15
Lagrangian
Lagrangian

L = K P

=
= = = =
+
+ =
N
1 i
i
i
i i
N
1 i
i
1 j
i
1 k
N
1 i
2
T
i i
r T m

2
1
)

T
Trace(
2
1

T
i a k j
k
i
j
g
q J q q
q
J
q
L
i

Marcelo H. Ang Jr.
16
Dynamic Equations
i i
q

-
q

dt
d

L L
F
i
=
)

T
Trace(
2
1

I )

T
Trace(
2
1

N
1 i
i
1 j
T
i i
N
1 i
a
i
1 k
T
i i
p

= =
= =
+
+ =
j
p
i
j
p k
k
i
p p
q
q
J
q
q q
q
J
q q
L
change to k



Marcelo H. Ang Jr.
17
)

T
Trace( )

T
Trace( )

T
Trace(
T
i i
T
T
i i
T
i i
j
i
k k
i
j k
i
j
q
J
q q
J
q q
J
q
= =
Note that
since for p > i, we have
0

T
i
=
p
q

= =
+ =
N
p i
a
i
1 k
T
i i
I )

T
Trace(

p
p k
p
i
k p
q q
q
J
q q
L

= =
+ =
N
1 i
a
i
1 k
T
i i
I )

T
Trace(

p
p k
p
i
k p
q q
q
J
q q
L


Marcelo H. Ang Jr.
18
used Trace (M) = Trace(M
T
)

)

T


T
Trace(
)

T


T
Trace(
I )

T
Trace(

dt
d
N
p i
i
1 k
i
1 m
T
i i
2
N
p i
i
1 k
i
1 m
T
i i
2
N
p i
a
i
1 k
T
i i
p

= = =
= = =
= =
+
+
+ =
m k
k
i
m p
m k
p
i
m p
p k
p
i
k p
q q
q
J
q q
q q
q
J
q q
q q
q
J
q q
L



Marcelo H. Ang Jr.
19

r

T
m
)

T


T
Trace(
2
1

)

T


T
Trace(
2
1

N
p i
i
i
i
i
N
p i
i
1 j
i
1 k
T
i i
2
N
p i
i
1 j
i
1 k
T
i i
2

=
= = =
= = =
+
+ =
p
T
k j
j
i
p k
k j
k
i
p j p
q
g
q q
q
J
q q
q q
q
J
q q q
L


Marcelo H. Ang Jr.
20
Interchanging dummy indices of summation j & k in
2
nd
term, & then combining with the first,

r

T
m
)

T


T
Trace(

N
p i
i
i
i
i
N
p i
i
1 j
i
1 k
T
i i
2

=
= = =
+
=
p
T
k j
k
i
j p p
q
g
q q
q
J
q q q
L

Marcelo H. Ang Jr.
21
Therefore,

=
= = =
= =
+ +
= =
N
p i
i
i
i
i
N
p i
i
1 k
i
1 m
T
i i
2
N
p i
a
i
1 k
T
i i
p
r

T
m -
)

T


T
Trace(
I )

T
Trace(
F

dt
d
p
p
T
m k
p
i
m k
p k
p
i
k
p p
q
g
q q
q
J
q q
q q
q
J
q
q
L
q
L



Marcelo H. Ang Jr.
22
OR
Exchanging dummy summation indices p and i for i and j,

=
= = =
= =
+
+ =
N
i j
j
j
j
j
N
i j
j
1 k
j
1 m
T
j j
2
N
i j
a
j
1 k
T
j j
i
r

T
m -
)

T


T
Trace(
I )

T
Trace( F
i
i
T
m k
i
j
m k
i k
k
j
k
q
g
q q
q
J
q q
q q
q
J
q


Marcelo H. Ang Jr.
23
Model of Robot Dynamics
i
p
p
i
p
N
i p
T
p i
N
k j , i, max p
T
i
p
p
k j
p
2
ij k
N
j i, max p
T
i
p
p
i
p
ij
N
1 j
i
N
1 k
k j ij k i a
N
1 j
j ij i
q
P

r
q
T
g m - G
)
q
T
J
q q
T
Trace( C
)
q
T
J
q
T
Trace( D
where
G q q C q I q D F
i
=
=
=
=
+ + + =

=
=
=
= = =

Inertial
Centripetal +
Coriolis
Gravity
Marcelo H. Ang Jr.
24
OR
F = D(q) q + C(q, q) + G(q)
matrix-vector form

= = =
=
(
(

=
=
=
N
1 i
T
i
i
1 j
i
1 k
i
N
1 i
i
i
i i

T
Trace
2
1
K
r T m - P
D
2
1
K
k j
j
i
j
T
T
q q
q
J
q
g
q q (Kinetic Energy)
(Potential Energy)

Note the diff indices ( p = max(i, j) ) compared to
previous eqn for F
i
can be verified to be correct

Marcelo H. Ang Jr.
25
Simplifying the Dynamic
Model
Notation:
Define
p j for 0
p j .....A A .....Q A A

T
U
p j j 2 1
p
pj
> =
s = =
j
q
A
otherwise 0
p k j, ...A A .....Q A .....Q A A
q q
T
U
P k k j j 2 1
k j
p
2
pj k
=
s = =
A
Marcelo H. Ang Jr.
26
where
|
|
|
|
|
.
|

\
|
=
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 - 0

R
Q
Rotational joint
|
|
|
|
|
.
|

\
|
=
0 0 0 0
1 0 0 0
0 0 0 0
0 0 0 0

T
Q
Translational joint
Marcelo H. Ang Jr.
27
Summary

=
=
=
=
=
=

N
) k j , i, ( max p
T
pi pj k ij k
N
i p
p
p
pi p i
N
) j i, ( max p
T
pj pj ij
} U U { Trace C
r U m - - G
} U U { Trace D
p
T
p
J
g
J
Marcelo H. Ang Jr.
28
In general
F = D(q) q + C(q, q) + G(q) + b(q, q)
where actuator inertia I
a
i
are lumped into
D(q) : d
ii
d
ii
+ I
a
i
b(q, q): frictional force/torque vector
(damping + coulomb friction)
OR
F = D(q) q + H(q, q)
H(q, q) = coupling vector that incorporates
C(q, q), G(q) & b(q, q)





A more realistic dynamic model
Marcelo H. Ang Jr.
29
Actuator Dynamics
Ref: 1. Fu, Lee Gonzalez
2. Spong & Vidyasagar
relates
Applied Joint
Torque
with
Actuator Inputs
U = [ U
i
(t) ]
(Voltage)
+
_
V
L R
V
b
+
_
J
eff
t
m
u
m
Effective inertia
Effective viscous
Coeff. Of friction
(Damping)
B
eff
Marcelo H. Ang Jr.
30
Actuator Dynamics
t
m
B
m
u
m
J
m
u
L
t
L
B
L
J
L
Gear Ratio: N
Torque from = Torque on + Torque on Load
motor shaft motor Referred to motor
shaft
u
m
= N u
L

u
m
= N u
L

u
m
= N u
L

t
L
referred to motor shaft = t
L
*
t
L
*
= t
L

N
1


t
m
= (J
m
u
m
+ B
m
u
m
) + t
L

Mechanical Eq.
N
1
Marcelo H. Ang Jr.
31
Actuator Dynamics
Electrical Eqns:
b
V
dt
di
L Ri V + + =
where V
b
= K
E
u
m
(Back Emf)

K
E
in (Voltage constant of motor)

t
m
= K
T
i (torque delivered by motor)
K
T
in (Torque constant of motor)

rad/sec
volts
A
M - N
rad/sec
volts
(Note that if K
E
is in and K
T
is in ,

they are numerically equal, i.e. K
E
= K
T
)
rad/sec
volts
A
M - N
Marcelo H. Ang Jr.
32
Actuator Dynamics
Summary:
V - V R
dt
di
L
i K
N
1
B J
b i
T m m
= +
= + + =
L m m m


K
T
K
E
E
E
R Ls
1
+
m m
B s J
1
+
s
1
+
_
V(s)
V
b
I
t
m
L

N
1
+
u
m
u
m

Marcelo H. Ang Jr.
33
Actuator Dynamics
For the case when load is just an inertia load: J
L
,
t
L
= J
L
u
L
+ B
L
u
L
then

m m m
L m
m m m



|
.
|

\
|
+ +
|
.
|

\
|
+ =
|
.
|

\
|
+ + + =
L
2
m L
2
m
L L m m
B
N
1
B J
N
1
J
N
B
N
J
N
1
B J






J
eff
B
eff
t
m
= J
eff
u
m
+ B
eff
u
m
on motor side
where
L
2
m eff
L
2
m eff
B
N
1
B B
J
N
1
J J
+ =
+ =

Marcelo H. Ang Jr.
34
Actuator Dynamics
K
T
K
E
E
R sL
1
+
eff eff
B s J
1
+
s
1 +
_
V(s)
I
t
m
u
m
u
m

N
1
u
L
Marcelo H. Ang Jr.
35
Actuator Dynamics
For Electro Mechanical Systems, it is assumed that the
electrical time constant L/R is much smaller than the
Mechanical time constant J
m
/B
m

Setting L/R = 0 results in
K
E
E
E
R
K
T
m m
B s J
1
+
s
1
+
_
V
L

N
1
+
u
m
+
_
( )
m
m
m

K - V
R
K
K
R
K - V
K
R
V - V
i.e.
E
T
T
E
T
L
=
|
.
|

\
|
=
|
.
|

\
|
=


Marcelo H. Ang Jr.
36
Actuator Dynamics
The robot dynamics equations incorporating actuator
dynamics (ignoring L), in terms of robot (side) joint coords
are:
( ) ( )
|
.
|

\
|
=
+ + + + +

= =
N
K - V
R
K

N
1

N B b G C N J d
E
T
2
m i i
N
1 j
N
1 k
ij k
2
m ii
L
i k j i

q q q q


Torque delivered on motor side
Marcelo H. Ang Jr.
37
Actuator Dynamics
( )
V
NR
K

R N
K K
N B b G
C N J d
T
2
T E
2
m i i
N
1 j
N
1 k
ij k
2
m ii
=
|
.
|

\
|
+ + +
+ + +

= =
i
k j i
q
q q q



Note J
m
, B
m
, K
E
, K
T
, N, R are different for each
actuator-drive system
Marcelo H. Ang Jr.
38
Summary of Properties &
Characteristics of Inertia Coefficients
Inertia Matrix
Symmetric : d
ij
= d
ji
for all i and j
Positive definite : d
ii
> 0 for all i
d
ij
2
< d
ii
d
jj
for all i = j

Self Inertia Coefficient (i = j)
joint i translational:
d
ii
= m
i
+ + m
N
= constant
joint i rotational:
d
ii
= (m
i
+ + m
N
)K
ii
2
= f(q
i+1
, , q
N
)
Marcelo H. Ang Jr.
39
Mutual Inertia Coefficients (for j > i)
i = Transl, j = Transl:
d
ij
= (m
j
+ + m
N
) (z
i-1
z
j-1
) = f(q
i+1
, , q
j-1
)
i = Transl, j = Rot:
d
ij
= (m
j
+ + m
N
) (z
i-1
x z
j-1
) p
j
= f(q
i+1
, , q
N
)
i = Rot, j = Transl:
d
ij
= (m
j
+ + m
N
) (z
j-1
x z
i-1
) p
i
= f(q
i+1
, , q
j-1
,
q
j+1
, , q
N
)
i = Rot, j = Rot:
d
ij
= (m
j
+ + m
N
) k
ij
= f(q
i+1
, , q
N
)
Coeff of Coupling
j i all for
) d (d
d
k
2
1
j j ii
ij
ij
= =
Summary of Properties &
Characteristics of Inertia Coefficients
Marcelo H. Ang Jr.
40
Centrifugal And Coriolis
Coefficients
C
ijm
(Centrifugal & Coriolis coefficients) are
related to the inertial matrix through the
christoffel symbols:

c
c
c
c
+
c
c
=
i
j m
j
im
m
ii
ij m
q
d
-
q
d

q
d

2
1
C
These Christoffel symbols coupled with the
functional dependence of the inertial coef:
d
ij
= f(q
s+1
, , q
N
) where s = min(i, j)
simplify the formulation of the Centrifugal &
Coriolis coefs.
Marcelo H. Ang Jr.
41
Centrifugal And Coriolis
Coefficients
j) min(i, m for 0

d
since : l Centrifuga
ij
s =
m
q
C
ijj
=
j i for

d

2
1
j j
>
i
q
0 for i = j
j i for

d
ij
<
j
q
There are therefore N(N-1) independent
centrifugal coefficients
Marcelo H. Ang Jr.
42
Centrifugal And Coriolis
Coefficients
Furthermore
C
ijj
= f(q
s+1
, , q
N
) where s = min(i, j)
Note that C
ijj
characterizes the effect of the velocity
of joint j on link i
C
jjj
= 0 of course
C
ijj
is introduced by the coordinate dependence of the
inertial coefs. d
ij
when i > j,
C
ijj
= 0 if d
jj
is independent of q
i
when i < j,
C
ijj
= 0 if d
ij
is independent of q
j
Marcelo H. Ang Jr.
43
Centrifugal And Coriolis
Coefficients
Ex: Transl. joint j d
jj
= constant,
d
ij
= independent of q
j
C
ijj
= 0

translational joint does not exert centrifugal
forces on any link

centrifugal force is thus a unique characteristic
of a rotational joint
Marcelo H. Ang Jr.
44
Centrifugal And Coriolis
Coefficients
Coriolis Coefficients:
Based again upon the functional dependence of the inertial
coefficients, the coriolis coefs C
ijm
for j = m are:
C
ijm
=

i and m j for

d
-

d

2
1
j m ij
<
)
`

i m
q q
i and j m for

d
-

d

2
1
j m
im
<

i j
q q
m and j i for

d
-

d

2
1
im
ij
<

j m
q q
= f(q
s+1
, , q
N
) where s = min(i, j)
Marcelo H. Ang Jr.
45
Centrifugal And Coriolis
Coefficients
The symmetry of the inertial coefs leads to the
symmetry of the Coriolis coefs:
C
ijm
= C
imj
C
ijm
characterizes the effect of the coupling of
velocities of links j and m on link i
There appears to be N
2
(N-1)/2 independent
Coriolis coefs
This number is reduced to N(N-1)(N-2)/3 by
incorporating the non-interacting & reflective
coupling properties of the Coriolis coefs.
Non-Interacting C
ijj
= 0 for all j s i
Reflective Coupling C
ijm
= - C
mji
for all j s i, m
Marcelo H. Ang Jr.
46
Centrifugal And Coriolis
Coefficients
Non-interacting property states that there is no Coriolis
force on a link due to the coupling of its velocity with
the velocity of an inner link.
Reflective coupling property states that the Coriolis coef
for link i, due to the coupling of velocities of links
j and m (j s i, m), is equal & opposite to the Coriolis coef
for link m due to the coupled velocities of links j & i.
Marcelo H. Ang Jr.
47
Centrifugal And Coriolis
Coefficients
Coriolis forces account for the difference between
forces measured in rotating & non-rotating coord frames.
This diff. results from the observation that the energies of
a particle moving in 2 frames are diff. The rotation of
the first frame adds to a component which depends on the
coordinates of the particle & is proportional to angular
velocity of rotating frame.
Marcelo H. Ang Jr.
48
Centrifugal And Coriolis
Coefficients
The practical significance of this fact is that a pair of links,
the inner of which is translational, does not exert Coriolis
forces on any link.
Consequently, the Coriolis force is a unique characteristic
of the coupling of the velocity of a rotating link to the
velocity of an outer link.
Marcelo H. Ang Jr.
49
Gravitational Coefficients
G
i
= m
i
{ o
i
(g z
i-1
) + o
i
*
(g (z
i-1
x p
i
)) } + .. +
m
n
{ o
i
(g z
n-1
) + o
i
*
(g (z
n-1
x p
n
)) }, for i,i+1,i+2,N
dot product
For o
i
= 0 (Rot. Joint), G
i
= G
i
(q
1
, , q
N
)
For o
i
= 1 (Trans. Joint), G
i
= G
i
(q
1
, , q
i-1
)

There are 2 configurations of Practical Significance:
1) If link i translates in a direction to gravity field,
G
i
= 0 since g z
i-1
= 0 (cylindrical robot last link)
2) If link i rotates about an axis parallel to gravity field,
G
i
= 0 since g (z
i-1
x p
i
) = 0 (SCARA, 1
st
2 links)
Marcelo H. Ang Jr.
50
Gravitational Coefficients
Hence, the gravitational coef of a joint characterizes the
work production of that joint in the gravity field.

In practice, counterbalances are introduced to reduce the
effect of gravity
A counterbalance is added to a link to alter its
mass distribution and reduce the corresponding
gravitational coef.
Marcelo H. Ang Jr.
51
Summary
Dynamics
L = K P

dt
d
=
i i
q
L
q
L
I
i
(t)

I
i
(t) =
i i
N
1 j
N
1 m
ij m
N
1 j
ij
G C d + + +

= = =
m j j
q q q

Vector-Matrix Form
I = D(q) q + C(q, q) + G(q) + t(t)
N x 1 N x 1 N x 1 N x 1 N x 1 N x N

J
T
F
(STATICS)
Marcelo H. Ang Jr.
52
Summary
Inertia

=
=
=
=
N
i k
k ki k i
N
) j i, ( max k
T
ki kj ij
r U m - G
} U U { Trace d
T
k
g
J
expressed in local link frame
expressed in base frame
Marcelo H. Ang Jr.
53
Summary
(
(
(
(
(
(
(
(
(

+
+
+ +
=
k k k k k k k
k k
kzz kyy kxx
kyz kxz
k k kyz
kzz kyy kxx
kxy
k k kxz kxy
kzz kyy kxx
m z m y m x m
z m
2
I - I I
I I
y m I
2
I I - I
I
x m I I
2
I I I -

k
J
= x
2
dm
= y
2
dm
= z
2
dm
I
xx
= (y2 + z2) dm

I
yy
= (x2 + z2) dm

I
zz
= (y2 + x2) dm
mx = x dm
expressed in local link frame
Marcelo H. Ang Jr.
54
Centripetal and Coriolis
Coefficients
C
ijm
=

i and m j for

d
-

d

2
1
j m ij
<
)
`

i m
q q
i and j m for

d
-

d

2
1
j m
im
<

i j
q q
m and j i for

d
-

d

2
1
im
ij
<

j m
q q
= f(q
s+1
, , q
N
) where s = min(i, j)
Marcelo H. Ang Jr.
55
Summary
Properties of Inertial Coeffs:
1) d
ij
= d
ji
Symmetry for all i, j
2) d
ii
> 0 for all i
3) d
ij
2
< d
ii
d
jj
for all i = j
4) d
ij
= f(q
s+1
, , q
N
) s = minimum(i, j)
5) d
NN
= constant (last link)
= mass of last link (if translational)
= moment of inertia of last link (if rotational)
6) d
ii
= m
i
+ m
i+1
+ + m
N
if joint i is translational
= total mass translated by joint i
Marcelo H. Ang Jr.
56
Summary
i j for 0
i j .....A A .....Q A A

T
U
i j j 2 1
i
ij
> =
s = =
j
q
A
otherwise 0
i k j, A .....Q A .....Q A A

T
U
k k j j 2 1
i
2
ij k
=
< = =
k j
q q
Marcelo H. Ang Jr.
57
Summary
|
|
|
|
|
.
|

\
|
=
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 - 0

R
Q
|
|
|
|
|
.
|

\
|
=
0 0 0 0
1 0 0 0
0 0 0 0
0 0 0 0

T
Q
|
|
|
|
|
.
|

\
|
=
0 0 0 0
0 0 0 0
0 0 1 - 0
0 0 0 1 -

2
R
Q
Properties of the Q matrices:
1) Q
T
Q
T
= 0
2) Q
R
T
= -Q
R

3) Q
R
Q
T
= Q
T
Q
R

4) Q
T
A = Q
T
any Transformation matrix
Marcelo H. Ang Jr.
58
Rigid Body Dynamics
Newton-Euler
A mass in pure translation has momentum

m v
G
= linear momentum
velocity (translational) of
center of gravity
The Net Force that causes the motion is
G G G
a v v F m m ) (m
dt
d
= = =

Marcelo H. Ang Jr.
59
Rigid Body Dynamics
Newton-Euler
Angular Momentum (or Moment of Momentum) of a
Body with respect to an arbitrary point A (moving or
non-moving) is
}
=
m(t)
dm ) - ( ) - ( (t)
A A A
p p p p H



z
0
x
0
A
dm
Body
A
p
p
A
p - p
of arbitrary
pt. of Body
p = position &
p = velocity

Marcelo H. Ang Jr.
60
Rigid Body Dynamics
Newton-Euler
Let G = center of mass of Body
Let P
A
(t)
P
G
(t)
be position vectors describing A & G &
change with time (moving)
then
H
A
(t) = (p
G
p
A
) x m (p
G
p
A
) + H
G
velocities
(follows from definition of angular momentum)

Marcelo H. Ang Jr.
61
Rigid Body Dynamics
Newton-Euler
Look at H
G
(t) , G = center of mass
H
G
(t) = (p p
G
) x (p p
G
) dm


Express all vectors in the Body frame G


G
Body frame
x
G
y
G
dm
p
p
G
= 0 p
G
= 0

H
G
(t) = p x p dm

p = w x p



Marcelo H. Ang Jr.
62
Rigid Body Dynamics
Newton-Euler
H
G
(t) = p x (w x p) dm

p x (w x p) = ( ) - ( )

= (p
x
2
+ p
y
2
+ p
z
2
) - (p
x
w
x
+ p
y
w
y
+ p
z
w
z
)

= (p
y
2
+ p
z
2
) w
x
p
x
p
y
w
y
p
x
p
z
w
z


= (p
x
2
+ p
z
2
) w
y
p
x
p
y
w
x
p
y
p
z
w
z


= (p
x
2
+ p
y
2
) w
z
p
x
p
z
w
x
p
y
p
z
w
y

p p
w
w
w
p p
p
Marcelo H. Ang Jr.
63
Rigid Body Dynamics
Newton-Euler
(
(
(

(
(
(
(

+
+
+
=
z
y
x
2
y
2
x z y z x
z y
2
z
2
x y x
z x y x
2
z
2
y
w
w
w

p p p p - p p -
p p - p p p p -
p p - p p - p p
) ( p w p
Marcelo H. Ang Jr.
64
Rigid Body Dynamics
Newton-Euler

H
G
(t) = p x (w x p) dm
(
(
(

(
(
(
(

+
+
+
=
} } }
} } }
} } }
z
y
x
2
y
2
x z y z x
z y
2
z
2
x y x
z x y x
2
z
2
y
w
w
w

dm p p dm p p - dm p p -
dm p p - dm ) p (p dm p p -
dm p p - dm p p - dm ) p (p

I
G
(inertia Tensor)
Marcelo H. Ang Jr.
65
Rigid Body Dynamics
Newton-Euler
I
G
= calculated with respect
to a reference frame
with origin = center of mass
& that w is with respect
to this frame!

(
(
(

=
zz yz xz
yz yy xy
xz xy xx
I I - I -
I - I I -
I - I - I

G
I
I
xx
= (y
2
+ z
2
) dm
I
yy
= (x
2
+ z
2
) dm

I
xy
= xy dm
H
G
(t) = I
G
w(t)
Marcelo H. Ang Jr.
66
Rigid Body Dynamics
Newton-Euler
Different Orientation of Body Frames whose
origin is at center of mass

y
G
x
G
O
G
= O
A
= center of mass
I
A
=
A
R
G
I
G

G
R
A
Marcelo H. Ang Jr.
67
Rigid Body Dynamics
Newton-Euler


C
B
{A}
Inertia Tensor
A
I
B


depends on a point. (e.g. B)
(Inertia about a point)

depends on a frame of expression for coordinates (e.g. A)
( x y z )

(This point need not be the origin of frame of expression)
Marcelo H. Ang Jr.
68
Transforming Inertia Tensors
Parallel Axis Theorem

G

A

z
y
x

G
G
G
|
|
|
.
|

\
|
=
G
A
P
= coordinates
of G
expressed in A
Frames A & G are parallel

G is at center of mass of Body
Marcelo H. Ang Jr.
69
Transforming Inertia Tensors
I
A
zz
= I
G
zz
+ m(x
G
2
+ y
G
2
) ; I
A
xy
= I
G
xy
mx
G
y
G


OR in Matrix Form:

I
A
= I
G
+ m[
A
p
G
T

A
p
G
I
3

A
p
G

A
p
G
T
]

I
3
= 3 x 3 Identity Matrix
Marcelo H. Ang Jr.
70
Eulers 2
nd
law (moment of momentum)
(angular momentum)

M
G
= moment acting about G to cause motion
M
G
G
w
w

| | (t)
dt
d

(t)
dt
d

w I
H M
G
G G
=
=
M
G
= I
G
w + I
G
w


because Frame G is also rotating
Marcelo H. Ang Jr.
71
I
G
= w x I
G

from definition of I
G

[ Note that p = w x p ]


M
G
= I
G
w + w x I
G
w

w = angular velocity of Body (frame G which is rotating)
expressed in some universe
form of reference
Marcelo H. Ang Jr.
72
Moment about a Point (A)

F p
A
M
A
= x
Expressed in some universe frame
p
F

G
{A} = Fixed
A
p
G
Body is moving
Marcelo H. Ang Jr.
73
H
A
(t) = p
G
p
A
x m(p
G
p
A
)
Express all vectors in {A},
A
p
G
= 0 , p
A
= 0 fixed
H
A
(t) =
A
p
G
x m(p
G
) + H
G
=
A
p
G
x m
A
v
G
+ H
G



velocity of G
A about Moment
dt
d
= =
A
A
M
H
acceleration
G G
A
G
A
G G
A
A
H v v a p
H

dt
d
m m
dt
d
+ + =
0
M
A
=
A
P
G
x ma
G
+ M
G

Marcelo H. Ang Jr.
74
Newton Euler
Equations of Motion



{i 1}
{G
i
}
{i}
n
i
f
i
s
i
-f
i+1
-n
i+1
G
i
p
i-1
-p
i
s
i
G
i
= center of mass
of link i
{o} = Base frame
Marcelo H. Ang Jr.
75
Let all vectors be expressed in Frame |

I
G
i
= Inertia matrix of link i about its center of mass
with respect to Frame 0 ( {G
i
} || Frame {0} )

f
i
= force exerted on link i by link i 1 at
Frame {i 1}

n
i
= moment exerted on link i by link i 1 at
Frame {i 1}


Marcelo H. Ang Jr.
76
s
i
= position of center of mass of link i from
origin of Frame {i} expressed in base Frame |

F
i
= total external force exerted on link i at center
of mass

N
i
= total external moment exerted on link i at
center of mass

Marcelo H. Ang Jr.
77
Then
F
i
= f
i
f
i+1

N
i
= n
i
n
i+1
+ (-s
i
x (-f
i+1
)) + (p
i-1
p
i
s
i
) x f
i
F
i
= m
i

o
a
G
i

,
o
a
G
i
= acceleration of center of mass
of link i, expressed in base frame

N
i
= I
G
i
w
i
+ w
i
x (I
G
i
w
i
)
Note: I
G
i
= change as robot moves, because it is
with reference to base frame.

Marcelo H. Ang Jr.
78
Also: To compute a
G
i

o
p
G
i
=
o
p
i
+
o
R
i

i
p
G
i


o
v
G
i
=
o
v
i
+
o
R
i

i
p
G
i
=
o
v
i
+
o
w
i
x (
o
R
i

i
p
G
i
)

o
a
G
i
=
o
a
i
+
o
w
i
x (
o
R
i

i
p
G
i
) +
o
w
i
x (
o
R
i

i
p
G
i
)



o
a
G
i
=
o
a
i
+
o
w
i
x
o
s
i
+
o
w
i
x (
o
w
i
x
o
s
i
)
=
o
R
i

i
p
G
i


Marcelo H. Ang Jr.
79
Recursively, equations are:
f
i
= F
i
+ f
i+1

= m
i
a
G
i
+ f
i+1
n
i
= n
i+1
+ s
i
x (-f
i+1
+ f
i
) + (p
i
p
i+1
) x f
i
+ N
i
n
i
= n
i+1
+ s
i
x F
i
+ (p
i
p
i+1
) x f
i
+ N
i
or if we let p
i
*
= p
i
p
i-1


n
i
= n
i+1
+ p
i
*
x f
i+1
+ (p
i
*
+ s
i
) x F
i
+ N
i
Marcelo H. Ang Jr.
80
Initial Conditions
gravity
z
o
y
o
x
o
(
(
(

+
=
9.8
0
0

o
v

positive because
To have all Moments acct. for gravity, it is like making
the masses accelerate upwards at 9.8 m/s
2
(to counter gravity)
w
o
= w
o
= v
o
= 0

v
o
= ( g
x
, g
y
, g
z
)
T
to include gravity
|g| = 9.8 m/s
2


Marcelo H. Ang Jr.
81
Note:

All vectors are referenced to base coordinate system,
including the
o
I
i
tensors,
Inertia about c.g. of link i but
computed with axis parallel to base.
changes as robot moves.

i-1
R
i
= Rotation matrix part of A
i
=
i-1
T
i


Transform all vectors into its own link frame

i
I
i
=
i
R
o

o
I
i

o
R
i
= constant
Marcelo H. Ang Jr.
82
Recursive Newton-Euler
equations of motion
Forward equations: i = 1, 2 , , n
w
i
=
w
i-1
+ z
i-1
q
i
if link i is rotational
w
i-1
if link i is translational

w
i
=
w
i-1
+ z
i-1
q
i
+ w
i-1
x (z
i-1
q
i
) if link i is rotational
w
i-1
if link i is translational




v
i
=
w
i
x p
i
*
+ w
i
x (w
i
x p
i
*
) + v
i-1
if link i is rotational
z
i-1
q
i
+ w
i
x p
i
*
+ 2w
i
x (z
i-1
q
i
)
+ w
i
x (w
i
x p
i
*
) + v
i-1
if link i is translational




a
i
= w
i
x s
i
+ w
i
x (w
i
x s
i
) + v
i

Marcelo H. Ang Jr.
83
Recursive Newton-Euler
equations of motion
Backward equations: i = n, n-1 , , 1
F
i
= m
i
a
i
N
i
= I
i
w
i
+ w
i
x (I
i
w
i
)
f
i
= F
i
+ f
i+1

n
i
= n
i+1
+ p
i
*
x f
i+1
+ (p
i
*
+ s
i
) x F
i
+ N
i

t
i
=
n
i
T
z
i-1
+ b
i
q
i
if link is rotational
f
i
T
z
i-1
+ b
i
q
i
if link is translational


where b
i
is the viscous damping coefficient for joint i.

The usual initial conditions are w
o
= w
o
= v
o
= 0 and
v
o
= ( g
x
, g
y
, g
z
)
T
(to include gravity), where |g| = 9.8062 m/s
2


Marcelo H. Ang Jr.
84
Efficient recursive Newton-Euler
equations of motion
Forward equations: i = 1, 2 , , n
i
R
o
w
i
=
i
R
i-1
(
i-1
R
o
w
i-1
+ z
o
q
i
) if link i is rotational
i
R
i-1
(
i-1
R
o
w
i-1
) if link i is translational

i
R
o
w
i
=
i
R
i-1
[
i-1
R
o
w
i-1
+ z
o
q
i
+ (
i-1
R
o
w
i-1
) x z
o
q
i
]
if link i is rotational
i
R
i-1
(
i-1
R
o
w
i-1
) if link i is translational




Marcelo H. Ang Jr.
85
Efficient recursive Newton-Euler
equations of motion
i
R
o
v
i
=
(
i
R
o
w
i
) x (
i
R
o
p
i
*
) + (
i
R
o
w
i
) x [(
i
R
o
w
i
)

x (
i
R
o
p
i
*
)]
+
i
R
i-1
(
i-1
R
o
v
i-1
) if link i is rotational

i
R
i-1
(z
o
q
i
+
i-1
R
o
v
i-1
) + (
i
R
o
w
i
) x (
i
R
o
p
i
*
)
+ 2 (
i
R
o
w
i
) x (
i
R
i-1
z
o
q
i
)
+ (
i
R
o
w
i
) x [(
i
R
o
w
i
)

x (
i
R
o
p
i
*
)] if link i is translational





i
R
o
a
i
= (
i
R
o
w
i
) x (
i
R
o
s
i
) + (
i
R
o
w
i
) x [(
i
R
o
w
i
) x (
i
R
o
s
i
)] +
i
R
o
v
i


Marcelo H. Ang Jr.
86
Efficient recursive Newton-Euler
equations of motion
Backward equations: i = n, n-1 , , 1
i
R
o
f
i
=
i
R
i+1
(
i+1
R
o
f
i+1
) + m
i
i
R
o
a
i
i
R
o
n
i
=
i
R
i+1
[
i+1
R
o
n
i+1
+ (
i+1
R
o
p
i
*
) x (
i+1
R
o
f
i+1
)]
+ (
i
R
o
p
i
* +
i
R
o
s
i
) x (
i
R
o
F
i
)
+ (
i
R
o
I
i
o
R
i
)(
i
R
o
w
i
) + (
i
R
o
w
i
) + [(
i
R
o
I
i
o
R
i
)(
i
R
o
w
i
)]
Marcelo H. Ang Jr.
87
Efficient recursive Newton-Euler
equations of motion
t
i
=
(
i
R
o
n
i
)
T
(
i
R
i-1
z
o
) + b
i
q
i
if link is rotational

(
i
R
o
f
i
)
T
(
i
R
i-1
z
o
) + b
i
q
i
if link is translational



where z
o
= (0, 0, 1)
T
and b
i
is the viscous damping
coefficient for joint i. The usual initial conditions are
w
o
= w
o
= v
o
= 0 and v
o
= ( g
x
, g
y
, g
z
)
T
(to include gravity),
where |g| = 9.8062 m/s
2.