Inverse Dynamics Calculation of Underactuated
Link Systems Using Parallel Solution Scheme
1

Daigoro Isobe1, Kouji Yamanaka2 and Yuto Kitamura3
Dept. of Engineering Mechanics and Energy, University of Tsukuba, Japan
isobe@kz.tsukuba.ac.jp
2
Kyushu Railway Company, Japan
3
Graduate School, University of Tsukuba, Japane0411288@edu.esys.tsukuba.ac.jp

Abstract- A general-purpose solution scheme of inverse dynamics
for link systems was developed on the basis of a finite element
approach. It can not only deal with different types ofconfigurations, such as open-loop, closed-loop, or multibranchlink systems, but also with the elasticity of constituted links
without the need to revise any part of the scheme. The main
objective of this study is to extend the use of the scheme by
applying the inverse dynamics calculations to several types ofunderactuated link systems. A solution scheme of kinematics is
also developed on the basis of the finite element method in orderto calculate target trajectories that compensate for the inertial
forces acting in the systems. The obtained trajectories are fed
into the inverse dynamics calculation using the parallel solution
scheme. The torque values are verified through comparison with
the input moment used in finite element analyses, and thevalidity of the parallel solution scheme is confirmed.

I.

INTRODUCTION

Dynamic equations used for feed-forward control ofrobotic mechanisms include interdependent variables between
the constituting links, since they are normally evaluated inrelative polar coordinates and in the dimension of torque.
Accordingly, it will become highly complicated to derive
inverse dynamics of closed-loop link systems, continuouslytransforming systems, or flexible link systems. Nakamura andGhodoussi proposed a systematic computational scheme of
the inverse dynamics specified for closed-loop link systems,
derived using d'Alembert's principle [1]. Sugimoto derived an
equation of motion for closed-loop link systems by motor
algebra [2]. Nakamura and Yamane developed acomputational algorithm for the inverse and forward
dynamics of open and closed kinematic chains, which can beapplied seamlessly to the motion of any rigid link system
without switching algorithms [3]. However, these methods
are specific for rigid-body link systems, and users
occasionally must revise the methods or dynamic equations
when the forms of the system, elasticity of the links, orcharacteristics of the joints (active or passive) are changed.
Isobe, on the other hand, developed a completely new
solution scheme for inverse dynamics, called the parallel
solution scheme [4]. The scheme was developed using a finite
element approach, handling the entire system as a continuum.
By taking advantage of the natural characteristics of the finitek,(((element method (FEM), i.e., the capability of expressing the
behavior of each discrete element as well as that of the entire
continuous system, local information, such as nodal forcesand displacements, can be calculated in parallel. The analyzed
model is evaluated in absolute Cartesian coordinates with theequation of motion expressed in the dimension of force. The
inverse dynamics is calculated using a matrix-form relation to
the nodal forces obtained by the finite element calculation.
The matrix-form equations are divided individually into termsof force, transformation between coordinates, and length,
which makes the scheme potentially higher in applicability
and expansibility. The scheme can not only deal withdifferent types of configurations such as open-loop, closedloop, or multibranch link systems, but also with the elasticity
of constituent links without the need to revise any part of thescheme.
There are two different versions of the parallel solution
scheme for calculating the inverse dynamics of link systems.
One version uses two beam element subdivisions per link to
explicitly express the center of gravity of links, and the otheruses only one beam element per link with consistent mass
distribution. The former version is easier to compare withconventional dynamic equations since it explicitly expresses
the center of gravity. We implemented the linear Timoshenko
beam elements in modeling with the former version [4]. The
latter, on the other hand, has the merits of high accuracy,
short calculation time and stability, particularly for flexible
models [5]. Therefore, the latter version with Bernoulli-Euler
beam elements for the modeling, is described and used in thispaper. It is also extended to underactuated link systems [6,7]by developing a kinematics solution scheme for the systems,
which is a revised version of the one used for flexible models.
Some numerical examples of underactuated systems areshown, and the results are verified by comparison with theresults obtained by finite element analyses.
II. PARALLEL SOLUTION SCHEME OF INVERSE DYNAMICS
In the parallel solution scheme, a link system consisting of
motor joints and links, as shown in Fig. 1(a), is modeled andsubdivided using finite elements. One method of modelingthe system is to subdivide a link into two beam elements, as
shown in Fig. 1(b), with an intermediate node explicitlyJoint (Servo motor)

Center ofgravity

Nodes

Nodes

+

Link

Beam elements

Beam elements+(a) Link system: Motor mass

: Motor mass

: Lumped mass

: Consistent mass(b) Lumped mass distribution(c) Consistent mass distribution

Fig. 1. Modeling of link systems using finite elements.

expressing the center of gravity [4]. Another method is tosubstitute a link with a single beam element, as shown in Fig.
1(c), assuming a consistent mass distribution along the link[5]. The type of modeling shown in Fig. 1(c) is described andused throughout this paper.
The order of the displacement function used for a
Bernoulli-Euler beam element is higher than that of a linear
Timoshenko beam element, and thus, the former can express
deformation more accurately with fewer elements. The
consistent mass matrix of the beam element is formulated in
the same manner as the displacement function and does not
require an expression for the center of gravity. Note that,since the deformation of the element is defined using ahigher-order function, it requires only one-element
subdivision per member for cases of infinitesimal
deformation.
Figure 2 shows the nodal forces (based on global
coordinates) acting on the ith link in a three-dimensional
open-loop n-link system with a consistent mass distribution.
The joint torque τix required around the x-elemental axis on
the ith link is determined by adding the i+1th joint torquePnPnτ(i+1)x to the sum of the moments of inertia acting on this link,
and is expressed by the nodal forces Fiy and Fiφx based onelemental (or link) coordinates asúix = li (j=i+1

fún g = [Ln ][T n ]fP n g ,Link i

FiûX

Z

(1)

(2)

where {Pn} is a 6n×1 vector related to nodal force and isdefined asn

fP g =

8
9P>< P12 >
=
>
:

Å
Å
Pn

>
;

; where fPi g =

8 Pn
9
>
Pjn=i+1 FjX >>>>< Pj =i+1 FjY >
=
>
>
>
:

n

j =i+1

FiûX
FiûY
FiûZ

FjZ

>
>
>
;

; (i = 1; ÅÅÅ; n).
(3)[Tn] is a 6n×6n transformation matrix and is defined asj=i+1 FjYFiûYFj )y + Fiûx + ú(i+1)x .

By considering the other components around the y- and zaxes and arranging them into global coordinates (X, Y, Z) in
matrix form, the joint torque vector is expressed asj=i+1 FjZ

FiûZ

n
X

Pn

Link i + 1n[T n ] = [hn ][TGE],yj=i+1 FjX

where [hn] is a correction matrix between the x-y and z-x
coordinate systems, which simply inverts the signs of thecomponents in the y-axis direction. [TnGE] is a transformation
matrix between the global and elemental coordinates and isexpressed aszYxElemental coordinate

X：NodesGlobal coordinate

Fig. 2. Nodal forces acting in a link system with consistent mass
distribution.

k,(((

(4)



2

6
6
6
6
h
i 6
6
n
=6
TGE
6
6
6
6
4

T2
T3
Å
0

h

h

Ai

i

2

7
7
7
7
7
7
7
7
7
7
7
5

0

where

and

3

T1

i

Ti

6
= 4 cosûiXy

cosûiXxcosûiXz

=

"

Å

Å

Ai

0

0

Ai

cosûiY xcosûiY ycosûiY zTn(5)[M]fu°
m gt+Å t +[M]fu°
d gt+Å t +[C]fu_d gt+Å t +[K]fÅud g =

,

(6a)
3

7
cosûiZy 5cosûiZz(6b),where φiXx, for example, represents the rotational angle
between the X (global) and x (elemental) coordinates.
[Ln] is a 3n×6n matrix related to link length and isexpressed as[Ln ] = [TÉn ][Én ] ,where [M] is the total mass matrix, [C] is the total damping
matrix, [K] is the total stiffness matrix, {F} is the external
force vector, and {R} is the internal force vector. The vectors
fu°
d g denote the acceleration vectors for the overallm g and fu°
system motion components and material deformation
components, respectively, fu_dg denotes the velocity vector
for material deformation components, and {∆ud} is theincremental material deformation vector at time t+∆t. By
applying Newmark’s β method (δ, β: integration parameters)
as a time integration scheme, the velocity and acceleration
vectors are calculated aséééfÅumgÄ( Ä1)fu_mgt Ä( Ä1)fu°m gt Åt (11a)
åÅt
å
2å
,
é
é
é
fu_d gt+Å t =fÅud gÄ( Ä1)fu_d gt Ä( Ä1)fu°d gt Åt
(11b)
åÅt
å
2å
,
1
1
1
fu_mgt Ä( Ä1)fu°
fu°fÅumgÄ
(11c)
m gt+Å t =
m gt
åÅt2
åÅt
2å
,
1
1
1
fu_d gt Ä( Ä1)fu°d gt (11d)fu°d gt+Å t =fÅud gÄ
åÅt2
åÅt
2å
,

fu_mgt+Å t =(7)

where [TnΛ] is a transformation matrix between eachelemental coordinate, and [Λn] is a matrix expressed as
2

6
6
6
6
h
i 6
6
n
=6
É
6
6
6
6
4

É2

Éi

7
7
7
7
7
7
7
7
7
7
7
5

0
É3

Å
0

where
h

3

É1

i

2

6
= 4 li
0
0

Å

Å

li

0

1 0

0

0

0 1

0

0

0 0

Én

where fu_m g is the velocity vector for the overall system
motion components and {∆um} is the incremental motion
vector at time t+∆t. Substituting (11) into (10) yields
(8)
([K]+

Ä[M](

.

To calculate the inverse dynamics for underactuated linksystems, information on target trajectories that compensate
for the inertial forces acting at the links, as well as on the
stiffnesses (or flexibilities) of the links and nonactuated joints

1
1
fu_d gt +( Ä1)fu°d gt )
åÅt
2å

1
1
1
fu_mgt Ä( Ä1)fu°fÅumgÄ
m gt )
åÅt2
åÅt
2å

é
é
+[C](( Ä1)fu_dgt +( Ä1)fu°d gt Åt)
å
2å

(9)

III. KINEMATICS CALCULATION FOR UNDERACTUATED
MODELSk,(((1é[C])fÅud g =
[M]+
2
åÅt
åÅt

fF gt+Å t ÄfRgt +[M](

,

3

7
0 5
1
0

, (10)

fF gt+Å t Ä fRgt#cosûiZx

are required. Therefore, a solution scheme of kinematics, thatwas developed on the basis of the FEM for flexible models[5], is revised and applied in this study.
When inertia caused by the overall system motion is taken
into account, the equation of motion at time t+∆t is derived
from the principle of virtual work as

.

(12)

By applying {∆um} as the input trajectory (normally an initial
trajectory for a rigid model) in a time integration loop of (12)and using the vectors of (11) at time t, {∆ud} at each time step
can be successively obtained. The displacement vector {um}for the overall system motion components and thedisplacement vector {ud} for the material deformation
components are calculated incrementally asfumgt+Å t = fumgt + fÅumg ,

(13a)

Y

(13b)

fud gt+Å t = fud gt + fÅud g .

The total displacement {u} is obtained by summing both theoverall system motion and material deformation componentsasThe final target trajectory considering the effects of stiffnessand damping is obtained using (14).
The calculation process described above can be commonlyused for flexible and underactuated models. However, somespecial treatments should be carried out for the latter model.
First, we use an elemental stiffness matrix with only axial
components for nonactuated links, in order to exclude all thetransmission of bending moments at the nonactuated joints.
Second, the input trajectory must be successively revised
during the calculation process to avoid any accumulation ofnumerical errors due to large deformation.
The trajectory revision algorithm developed in this study is
explained using a simple example of a two-joint
underactuated link system. Let the initial trajectory without
considering any effect be a horizontal 2-second motion, as
shown in Fig. 3. J1 is an active joint, whereas J2 is a
nonactuated joint. The parameters of the links are as follows:material of the links SUS430; flexural stiffness 20.84 Nm2;
link lengths 0.25 m; link mass 9.75 ×10-2 kg; surplus mass at
the tip 0.1 kg. If only the bending moment effect is excluded
from the nonactuated link, and no revision of the input
trajectory is performed, the final trajectory will become asshown in Fig. 4. It is clear that the coordinates are notsuccessfully transformed during the motion, since somelengthening of links can be observed in the figure. In order tosolve this problem, the concept of the successive revision of
0.6

Y[m]

Y[m]

0.4

0.4

0.2

J2

0.2

0

0

J1

J2

J1

-0.4
-0.6

-0.4
-0.6
-0.2

X[m] -0.8
0

0.2

0.4

Fig. 3. Initial trajectory.

0.6

-0.2

θt-1

Link 2

θt

θt-1
t+1

J2
φt-1

Link 1

θt+1
φt
φt+1

J1

θt

Initial trajectory
Input trajectory at each time
step
Revisedtrajectory

X
Fig. 5. Concept of successive trajectory revision algorithm
for underactuated link systems.

input trajectories, as shown in Fig. 5, is developed. The
broken lines in the figure are the initial trajectory, whereas
the gray lines are the input trajectory at each time step anddark lines are the revised trajectories at the end of each time
step. φ is the rotational angle of the active joint (J1, in thiscase), which will not be revised by the algorithm, and θ is therotational angle of the nonactuated joint (J2, in this case),
which will be calculated using the deformation obtained from(13b). Suppose the posture at time step t-1 is represented by
the dark line shown in the figure, with the rotational angle ofJ1 being φt-1. The input trajectory at the next time step t iscalculated only by rotating the active joint to φt, whereas theoverall posture is fixed as it is. The deformations of the linksystem at time step t are calculated starting from this posture(gray line at time step t). The calculated result at the end oftime step t will become as the dark line (with the rotational
angle of J1 being φt), and the revisions proceed successively
to the next time step t+1 in the same manner. In general nlink cases, the input trajectory can be revised by applying theabove algorithm from the base joint to the end joint
successively at each time step.
IV. NUMERICAL EXAMPLES

-0.2

-0.2

t

(14)

fugt+Å t = fumgt+Å t + fud gt+Å t.

0.6

t-1

X[m]
0

0.2

0.4

Fig. 4.
Final trajectory
performing successive revision.

k,(((0.6
without

Some numerical examples on underactuated link systems
are shown in this section. Figure 6(a) shows the calculated
trajectory of a two-joint underactuated model using thealgorithm described in the previous section. The initial
trajectory is given as shown in Fig. 3, and the same linkparameters as those in the previous section are used. The
integration parameters for Newmark’s method are given asδ=1/2 and β=1/4 in this example. The time increment for the
calculation is set to 10 ms. Note that the trajectory in Fig. 6(a)is improved compared with that in Fig. 4. Figure 6(b) shows
the torque curves calculated using data in Fig. 6(a) as input7(b) is due to both the initial velocity and acceleration being
given as those of a rigid link system without a nonactuatedjoint, and the modeling error arising as a result of notconsidering the stiffness of Link 1.
Next, the calculated torque curves are verified by adifferent process. A bending moment is applied to base joint
J1 of the two-link system and the motion is calculated by amechanical analysis with the FEM. Then, the obtained
trajectory is fed into the parallel solution scheme and thecalculated torque curves are compared with the initial input
moment. Although the calculated torques include the effects
of internal forces occurring in the link system even though the
input moment does not, the two curves should match well if
the motion is not too fast. The model described previously isused in this analysis. The time increment for the calculation is
set to 10 ms. Integration parameters for Newmark’s method
are given as δ=5/6 and β=4/9 in this example, to apply
numerical dissipation and to avoid the occurrence of
unnecessary high-order noise in the results [8]. Figure 8(a)shows the trajectory when an input moment was fed into thefinite element analysis, and Fig. 8(b) shows the input moment
and the calculated torque values. The two curves are in good
agreement and again, it can be observed that no torque isrequired at the nonactuated joint, J2. Slight overshoots andundershoots observed in the calculated torque curve of J1
oroginate from the consideration of internal forces in the linksystem.
Lastly, a numerical example for an eight-joint
underactuated link system is shown. The parameters of thelinks are as follows: material of the links SUS430; flexural
stiffness 20.84 Nm2; link lengths 0.1 m; link mass 4.95 ×10-2
kg; surplus mass at the tip 0.01 kg. The time increment forthe calculation is set to 5 ms and numerical dissipation isconsidered (δ=5/6 and β=4/9). The joints are numbered from

Y[m]
0.4
J1
J2

J2

0.05

0

Torque[N*m]

0.2

J1

0

-0.2
X[m]
0

-0.05
0

0.2

Fig. 6(a). Calculated trajectory
for a two-joint underactuatedlink system considering thestiffness of Link 1.

0.5

1
Time[s]1.5

2Fig. 6(b). Torque curves for a twojoint underactuated link system
considering the stiffness of Link 1.

Y[m]
0.4

J1
J2

J2

0.05

0

Torque[N*m]

0.2

J1

0

-0.2
X[m] -0.05
00.2

Fig. 7(a). Calculated trajectory
for a two-joint underactuatedlinksystem
without
considering the stiffness ofLink 1.

0

0.5

1Time[s]1.5

2Fig. 7(b). Torque curves for a twojoint underactuated link system
without considering the stiffness ofLink 1.

Y[m]0.5k,(((



0.4

0.015

Input moment
Calculated torque (J1)
Calculated torque (J2)

0.3

0.01

J2
Torque[N*m]for the parallel solution scheme described in Section III. The
torque curve for J2 retains the value of 0 throughout the
motion, which is proof that the joint is successfully expressed
as a nonactuated joint. The vibration seen in the torque curvefor J1 is proof that Link 1 has stiffness (or flexibility). It
vibrates with a natural period in the range of 0.025 s ~ 0.04 s(note that the natural period changes with the posture), whichmatches the theoretical value for a two-link free vibration
beam, 0.031 s. The vibration, however, generally exerts anadverse effect on the stability in actual control. Therefore, theproperty of Link 1 is idealized to that of a rigid link byneglecting the velocity and the acceleration of material
deformation components in (12), f u_d g and f u°d g , and theeffect is verified. Figure 7(a) shows the calculated trajectoryand Fig. 7(b) shows the torque curves obtained by applying
the parallel solution scheme to this case. Although there is novisual difference between Fig. 7(a) and Fig. 6(a), thevibration in the torque curve for J1 is clearly reduced in Fig.
7(b). The slight rise observed near the starting point in Fig.

0.2

0.005

0.1

0

J1

0

X[m]
0

0.05

0.1

0

0.5

1

1.5

2

Time[s]Fig. 8(a). Trajectory for a twojoint underactuated link system
obtained by finite element
analysis.

Fig. 8(b). Input moment and torquecurves for a two-joint underactuatedlink system. Input moment is applied
only to J1.

V. CONCLUSIONY[m]The parallel solution scheme of inverse dynamics using
high-order beam elements for modeling the link systems wasdeveloped, and the use of the scheme was extended tounderactuated systems in this work. There are no special
requirements when applying the scheme to the systems,
except for the calculation of the correct trajectories. The
trajectories can be calculated using the successive revisionalgorithm described in this paper, and basically, the use of theparallel solution scheme is not limited by the constituent
number of links. Some feed-forward control experiments ofunderactuated link systems are now in progress.

0.8

0.6

0.4

Input moment
J1
J2
J3
J4
J5
J6
J7
J8

Torque[N*m]

0.004

J3
0.2
J2

0.003
0.002

ACKNOWLEDGMENT
This work was supported by Grant-in-Aid for Scientific
Research (C) No.19560250, MEXT and the Electro-Mechanic
Technology Advancing Foundation.

0.001
0

J1
0

X[m]
0

0.1

0.2

Fig. 9(a). Trajectory for aneight-joint underactuated linksystem calculated by finite
element analysis.

0

0.5

1

1.5

2

REFERENCES

Time[s][1]

Fig. 9(b). Input moment and torquecurvesforaneight-joint
underactuated link system. Input
moment is applied only to J1.

[2]

the base joint as J1, J2, …, J8. J1 is an active joint, and theother joints are all nonactuated joints. Figure 9(a) shows thecalculated trajectory and Fig. 9(b) shows the input moment
and torque curves. Although the required torque for J1 is verysmall in this case, the calculated torque matches the input
moment, and the torques for other nonactuated joints
maintain values nearly equal to 0.

[3]
[4]
[5]
[6]
[7]
[8]

k,(((

Y. Nakamura and M. Ghodoussi, “Dynamics computation of closedlink robot mechanisms with nonredundant and redundant actuators,”
IEEE Trans. Robotics and Automation, vol. 5, no. 3, pp. 294-302, 1989.
K. Sugimoto, “Dynamic analysis of closed loop mechanisms on thebasis vectors of passive joint axes,” J. Robotic Systems, vol. 20, no. 8,
pp. 501-508, 2003.
Y. Nakamura and K. Yamane, “Dynamics computation of structurevarying kinematic chains and its application to human figures,” IEEE
Trans. Robotics and Automation, vol. 16, no. 2, pp. 124-134, 2000.
D. Isobe, “A unified solution scheme for inverse dynamics,” Advanced
Robotics, vol. 18, no. 9, pp. 859-880, 2004.
D. Isobe and A. Kato, “Feed-forward control of flexible link systems
using parallel solution scheme,” Int. J. Robotics and Automation, 2008,
in press.
A. Jain and G. Rodriquez, “An analysis of the kinematics and dynamics
of underactuated manipulators,” IEEE Trans. Robotics and Automation,
vol. 9, no. 4, pp. 411-422, 1993.
T. Suzuki and Y. Nakamura, “Control of manipulators with free-joints
via the averaging method,” IEEE Int. Conf. Robotics and Automation,
pp. 2998-3005, 1997.
K.J. Bathe, Finite Element Procedures, Prentice Hall, 1996.