Inverse dynamics for a three-link planar chain

Marcos Duarte (2002)
http://www.usp.br/eef/lob/md
Inverse dynamics for a three-link planar chain
This text is part of an appendix in “Cesari, P., Shiratori, T., Olivato, P., Duarte, M. (2001) Analysis of
kinematically redundant reaching movements using the equilibrium-point hypothesis. Biological
Cybernetics, 84, 217-226.” and is also included in the book “Zatsiorsky, VM (2002) Kinetics of Human
Motion. Champaign, Human Kinetics”.
The following convention applies to the notation used in this paper:
Subscript i runs 1, 2, or 3 meaning shoulder, elbow, or wrist joint when referring to angles, joint
moments, or joint reaction forces; or meaning upper arm, forearm, or hand segment when
referring to everything else.
x i , y i refer to the position of the center of mass of segment i in the horizontal or vertical direction,
respectively
l i is the length of segment i
d i is the distance from the proximal joint of the segment i to its center of mass position
d S is the distance from the wrist joint to the point of application of the spring force
m i is the mass of segment i
I i is the moment of inertia of segment i
F xi , F yi are the joint reaction forces of joint i in the horizontal or vertical direction, respectively
F S is the spring force
T i is the joint moment of joint i
g is the gravitational acceleration
Based on the model used here (see figure 1) the following relation applies to angles α, β,
and θ:
α 1 =β 1 =θ 1 (-)
α 2 =θ 2 - α 1 β 2 =π−α 2 (1)
1

Marcos Duarte (2002)
http://www.usp.br/eef/lob/md
α 3 =θ 3 - α 2 - α 1 β 3 =π−α 3
α i and β i are the angles in the "joint space", β i is the internal angle of the joint i and α I the external
one; θ i is the angle of the joint i in the "segment space".
Figure 1. Model of the human body for the arm movement in the sagittal plane with the schematic
location of the initial and target position. In this figure the spring force accounts for the external force
on the hand (see the text).
In order to compute the equations of motion, the linear accelerations of the center of gravity
of each link taking into account the constraints imposed by the kinematics of the linkage and
starting from the shoulder joint as a fixed reference point were calculated by the first derivate of the
Jacobian, J, of the respective angular velocities; or in a formal matrix form:
T
T
T
[& x
&& y ] = J&
[ & α & α & α ] + J [ && α && α & α
] i = 1...3
(2)
i
i
i
1
2
3
i
1
2
3
The linear accelerations of the segment’s center of mass represented in figures 1 and 6 can
be derived in a explicit form by:
Upper arm:
x = d
(3)
1 1
cosα1
2
& x = −d
&& α sinα
+ & α cos )
(4)
1 1( 1 1 1
α1
y = d
(5)
1 1
sinα1
2

Marcos Duarte (2002)
http://www.usp.br/eef/lob/md
3
)
sin
cos
( 1
2
1
1
1
1
1 α
α
α
α
&
&&
&
&
−
= d
y (6)
Forearm:
)
cos(
cos 2
1
2
1
1
2 α
α
α +
+
= d
x l (7)
)
)(2
cos(
)]
cos(
cos
[
)
sin(
)]
sin(
sin
[
2
2
2
1
2
1
2
2
1
2
1
2
1
1
2
2
1
2
1
2
1
2
1
1
2
α
α α
α
α
α
α
α
α
α
α
α
α
α
α
α
&
&
&
&
l
&&
&&
l
&&
+
+
−
+
+
−
+
−
+
+
= −
d
d
d
d
x
(8)
)
sin(
sin 2
1
2
1
1
2 α
α
α +
+
= d
y l (9)
)
)(2
sin(
)]
sin(
sin
[
)
cos(
)]
cos(
cos
[
2
2
2
1
2
1
2
2
1
2
1
2
1
1
2
2
1
2
1
2
1
2
1
1
2
α
α α
α
α
α
α
α
α
α
α
α
α
α
α
α
&
&
&
&
l
&&
&&
l
&&
+
+
+
+
−
−
+
+
+
+
=
d
d
d
d
y
(10)
Hand:
)
cos(
)
cos(
cos 3
2
1
3
2
1
2
1
1
3 α
α
α
α
α
α +
+
+
+
+
= d
x
l
l (11)
)
2
)(2
cos(
)]2
cos(
)
cos(
[
)
cos(
)]
cos(
)
cos(
[
)]
cos(
)
cos(
cos
[
)
sin(
)]
sin(
)
sin(
[
)]
sin(
)
sin(
sin
[
3
2
3
1
3
2
1
3
2
1
3
2
1
3
2
1
2
2
3
3
2
1
3
2
2
3
2
1
3
2
1
2
2
1
3
2
1
3
2
1
2
1
1
3
3
2
1
3
2
3
2
1
3
2
1
2
1
3
2
1
3
2
1
2
1
1
3
α
α
α α
α
α
α
α α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
&
&
&
&
&
&
l
&
&
l
&
l
l
&&
&&
l
&&
l
l
&&
+
+
+
−
+
+
+
+
−
+
+
−
+
+
+
+
−
+
+
+
+
+
−
+
+
−
+
+
+
+
−
+
+
+
+
+
= −
d
d
d
d
d
d
d
d
x
(12)
)
sin(
)
sin(
sin 3
2
1
3
2
1
2
1
1
3 α
α
α
α
α
α +
+
+
+
+
= d
y
l
l (13)
)
2
)(2
sin(
)]2
cos(
)
sin(
[
)
sin(
)]
sin(
)
sin(
[
)]
sin(
)
sin(
sin
[
)
cos(
)]
cos(
)
cos(
[
)]
cos(
)
cos(
cos
[
3
2
3
1
3
2
1
3
2
1
3
2
1
3
2
1
2
2
3
3
2
1
3
2
2
3
2
1
3
2
1
2
2
1
3
2
1
3
2
1
2
1
1
3
3
2
1
3
2
3
2
1
3
2
1
2
1
3
2
1
3
2
1
2
1
1
3
α
α
α α
α
α
α
α α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
α
&
&
&
&
&
&
l
&
&
l
&
l
l
&&
&&
l
&&
l
l
&&
+
+
+
−
+
+
+
+
−
+
+
−
+
+
+
+
−
+
+
+
+
+
−
+
+
+
+
+
+
+
+
+
+
+
+
+
=
d
d
d
d
d
d
d
d
y
(14)
Based on the free body diagrams, the equations of motion in the sagittal plane were derived
by means of the Newton-Euler method.
Hand:

Marcos Duarte (2002)
http://www.usp.br/eef/lob/md
m & x
3 3
= F x 3
− F (15)
S
& y
= F m g
(16)
m3 3 y3
−
3
I && α = T
3
3
3
+ F
+ F
S
d
S
x3
d
3
sin( α + α + α ) − F
1
1
2
2
3
3
3
1
y3
sin( α + α + α ) − I && α − I && α
d
3
3
cos( α + α + α )
2
1
2
3
(17)
Forearm:
m & x
= F x
− F
(18)
2 2 2 x3
m & y
= F − F m g
(19)
2 2 y2
y3
−
2
I && α = T
2
2
2
+ F
− T + F
x2
d
3
2
x3
( l
1
2
sin( α + α ) − F
− d )sin( α + α ) − F
2
2
y2
d
2
1
2
1
y3
cos( α + α ) − I && α
2
( l
2
− d )cos( α + α )
2
1
2
1
2
(20)
Upper arm:
m & x
= F x
− F
(21)
1 1 1 x2
m & y
= F − F m g
(22)
1 1 y1
y2
−
1
I && α = T − T + F ( l − d )sinα
− F ( l
1
1
1
+ F
x1
1
2
x2
1
1
d sinα
− F
y1
1
d cosα
1
1
1
y2
1
− d )cosα
1
1
(23)
The joint moments can be straightforwardly found solving the set of equations (15-23) in a
top-down way (first the wrist moment and so on) once the inertial parameters and the kinematics
data are known (the accelerations in the set of equations (3-14) are found first). If the kinematics
data are time-series, the equations are implemented in any programming package and the timeseries
of the joint moments are obtained. In this paper all data processing was implemented in
Matlab. In a matrix-vector form (useful for viasualizing the moment components, coupling and
transfer effects) the joint moments can be represented as:
T = M ( α)
& α
+ v(
α,
& α)
+ G(
α)
+ T ( α)
(24)
or
ext
4

Marcos Duarte (2002)
http://www.usp.br/eef/lob/md
⎡T1
⎤ ⎡M
( α)
⎢ ⎥ ⎢
⎢
T2
⎥
=
⎢
M ( α)
⎢⎣
T ⎥⎦
⎢
3 ⎣M
( α)
1,1
2,1
3,1
M ( α)
M ( α)
M ( α)
1,2
2,2
3,2
M ( α)
M ( α)
M ( α)
1,3
2,3
3,3
⎤⎡
&& α1
⎤ ⎡v(
α,
& α)
⎥⎢
⎥
+
⎢
⎥
&&
⎢
α
2
⎥ ⎢
v(
α,
& α)
⎥
⎦
⎢⎣
&& α ⎥⎦
⎢
3 ⎣v(
α,
& α)
1
2
3
⎤ ⎡G(
α)
⎥
+
⎢
⎥ ⎢
G(
α)
⎥⎦
⎢⎣
G(
α)
1
2
3
⎤ ⎡T
⎥
+
⎢
⎥ ⎢
T
⎥⎦
⎢⎣
T
ext
ext
ext
( α)
1 ⎤
( α)
⎥
2
⎥
( α)
⎥
3 ⎦
(24a)
Where:
T is the vector of joint moments (3x1).
M (α) is the inertia matrix (3x3). Because inertia matrices are symmetric (I 1,2 =I 2,1 , I 1,3 =I 3,1 and
I 2,3 =I 3,2 ), only six elements of the matrix should be determined. The elements are the three
moments of inertia, I 1,1 , I 2, 2 and I 3, 3
α& & is the vector of angular accelerations (3x1).
v ( α , & α)
is the vector of centrifugal/Coriolis terms (3x1).
G (α) is the vector of gravity terms (3x1).
T
ext
is the vector of joint moments due to other external forces besides gravity; in this case,
represents the moment due to the spring force (3x1).
The motion equations were rearranjed in the above format and the correspondent terms are:
[ T T ] T
T = (25)
1 2
T3
[&&
α && α &
] T
& α
= (26)
1 2
α
3
M ( α)
1,1
= m d
1
3
2
1
+ m [ l
+ I
2
1
1
+ l
+ m ( l
2
2
2
+ d
2
3
2
1
+ d
1
2
2
+ 2l
l
+ 2l
d
2
1
2
2
cosα
) + I
1
2
cosα
+ 2l
d
3
2
cos( α + α ) + 2l
d
2
3
2
3
cosα
] + I
3
3
(27)
M ( α)
1,2
= m ( d
+ l
1
2
3
2
2
+ l
1
2
d
2
cosα
) + I
d cos( α + α ) + 2l
d
3
2
2
2
3
+ m [ l
3
3
2
2
+ d
cosα
] + I
3
2
3
+ l l
1
2
cosα
2
(28)
M ( α)
l + I
(29)
2
1 ,3
= m3[
d
3
+
1d
3
cos( α
2
+ α3)
+ l
2d
3
cosα3]
3
M ( α)
2,1
= m ( d
2
+ m [ l
3
2
2
2
2
+ l
+ d
1
2
3
d
2
+ l
cosα
) + I
1
2
2
l cosα
+ 2l
d
2
2
2
3
cosα
+ l d
3
1
3
cos( α + α )] + I
2
3
3
(30)
5

Marcos Duarte (2002)
http://www.usp.br/eef/lob/md
M ( α)
l + I
(31)
2
2 2
2 ,2
= m2d
2
+ I
2
+ m3(
2
+ d
3
+ l
2d
3
cosα
3)
3
M ( α)
l + I
(32)
2
2 ,3
= m3(
d
3
+
2d
3
cosα3)
3
M ( α)
l + I
(33)
2
3 ,1
= m3[
d
3
+
1d
3
cos( α
2
+ α
3)
+ l
2d
3
cosα
3]
3
M ( α)
l + I
(34)
2
3 ,2
= m3(
d
3
+
2d
3
cosα
3)
3
M ( α = m d + I
(35)
) 3,3
3
2
3
3
v(
α,
& α)
= −[(
m l d
1
3
2
1
1
− [ m l d
3
2
+ m l
3
2
1
l
)sinα
+ m l d
2
sin( α + α ) + m l d
3
2
3
2
3
3
1
3
2
sin( α + α )](2 & α & α + & α )
2
sinα
](2 & α & α + 2 & α & α + & α )
3
2
1
3
3
2
3
1
2
3
2
(36)
v(
α,
& α)
v(
α,
& α)
2
3
= [( m l l
3
3
− m d
3
3
1
3
2
1
l
= [ m l d
3
+ m l d
2
2
3
+ m
2
2
d l )sinα
+ m d l sin( α + α )] & α
2
sinα
(2 & α & α + 2 & α & α + & α )
3
2
2
1
1
sin( α + α ) + m l d
2
sinα
(2 & α & α + & α )
3
3
1
3
2
2
3
2
2
2
3
3
3
3
1
3
2
sinα
] & α
3
1
2
3
1
(37)
(38)
G(
α)
1
= m gd
1
+ m
3
1
g[
l
cosα
+ m
1
1
cosα
+ l
1
2
g[
l
2
1
cosα
+ d
cos( α + α ) + d
1
1
2
2
cos( α + α )]
3
1
cos( α + α + α )]
1
2
2
3
(39)
G α ) = m gd cos( α + α ) + m g[
l cos( α + α ) + d cos( α + α + )] (40)
(
2 2 2 1 2 3 2 1 2 3 1 2
α3
G α ) = m gd cos( α + α + )
(41)
T
(
3 3 3 1 2
α3
ext
(
1
=
S 1 1 S 2 1 2 S 3 S 1 2
α
3
α ) −F
l sinα
− F l sin( α + α ) − F ( d + d )sin( α + α + ) (42)
T α ) −F
l sin( α + α ) − F ( d + d )sin( α + α + )
(43)
ext
(
2
=
S 2 1 2 S 3 S 1 2
α3
T α ) −F
( d + d )sin( α + α + )
(44)
ext
(
3
=
S 3 S 1 2
α3
6