FORCE CONTROL OF LINK SYSTEMS USING
THE PARALLEL SOLUTION SCHEME
Daigoro Isobe
Graduate School of Systems and Information Engineering, University of Tsukuba
1-1-1 Tennodai Tsukuba-shi, Ibaraki 305-8573, Japan
isobe@kz.tsukuba.ac.jp
Shunsuke Sato
Nissan Motor Co., Ltd.
6-17-1 Ginza Chuo-ku, Tokyo 104-8023, Japandimension of torque. Accordingly, it becomes highly
complicated to derive inverse dynamics of closed
kinematic chains, or structurally transforming ones. Many
researchers have developed their own ways [1-3] to solve
the problem, by devising and implementing new schemes
to handle the dynamic equations. On the other hand, Isobedeveloped a completely different way [4] for calculating
inverse dynamics by using a finite element approach. The
scheme is named the parallel solution scheme, since it
computes nodal forces in parallel by using the equationsof motion expressed in the dimension of force, andconverts them to torque by using a matrix-form equation
separated into individual terms of nodal forces, coordinatetransformation, and member length. Therefore, thescheme can naturally deal with open- and closed-loop linksystems independently, as well as those that graduallytransform and change their dynamics. There is also noneed to revise the basic numerical algorithm of thescheme, regardless of the stiffness of the constituting linkmember, whether it is rigid or flexible [5].

Abstract
In this paper, force control of link systems considering
dynamical effects is carried out by using the parallel
solution scheme, which was previously proposed andsuccessfully applied to inverse dynamics calculations offlexible manipulators. Generally, inverse dynamics is
calculated by using dynamic equations, and control torquefor generated forces is calculated by using Jacobianmatrices derived from kinematic equations. Two different
sets of torque are summed in such cases. On the other
hand, the parallel solution scheme calculates both types of
torque in a single procedure, without using Jacobianmatrices. The calculation is enabled by solving theequation of motion in the dimension of force, and byformulating the relationship between the calculated forces
and joint torque in a matrix form, separated into terms ofdifferent parameters. Generated forces for the control canbe considered simply by adding the values into one of thecomponents in the equation, which is used for calculating
inverse dynamics. The validity of the scheme is verified,in this paper, by carrying out a force control experimenton a simple link system where both generated forces anddynamical effects should be considered.

In this paper, we will discuss such cases where force
control is required, as well as the dynamic compensationof the link system. Normally, control torque for generated
forces and the inverse dynamics are calculated in different
approaches; the former by using Jacobian matrices
derived from kinematic equations, and the latter by using
the dynamic equations. On the other hand, the former
torque values are calculated in the same manner and withthe same algorithm for calculating inverse dynamics inthe parallel solution scheme. It is possible since thescheme originally handles the mechanical behavior of linksystems in the dimension of force. The parallel solution
scheme handles two different sets of joint torque in asingle procedure, and additional derivation of Jacobianmatrices or dynamic equations is not required. The
validity of the scheme is verified by carrying out a force
control experiment on a quick motion, two-arm linksystem holding an object, where both generated forcesand dynamical effects should be considered.

Key Words
Force Control, Inverse Dynamics, Link Systems, Parallel
Solution Scheme

1. Introduction
Recent developments in robotics have increased therequirements for mobility of robots as well as complexity
in their tasks. One of the ways to maintain the stability ofquick motion tasks is to consider the dynamics
compensation by using dynamic equations. However, thegenerally used dynamic equations include variables thatare interdependent between the constituting links, since
they are evaluated in relative polar coordinates and in the447-030239

derived by the following equation;

2. The Parallel Solution SchemefÅf g = fRgt Ä fF gt +In this section, the formerly proposed parallel solution
scheme is briefly explained, by comparing the mainequation for calculating inverse dynamics with that ofconventional schemes. Readers should refer to papers [4]and [5] for further information. Additionally, handling ofgenerated forces in nodal force vector is discussed, and asimple numerical example is shown.
2.1 Comparison
Equationswith

Schemes

Using

[M](

Dynamic(1)

where fúg is the torque vector, [M ] the inertial force
matrix, fV g the centrifugal force and Coriolis force term
_ , and í°
vector, and fGg the gravity force term vector. í, í
within the parentheses are the relative variables of theangle, the angular velocity and the angular acceleration
between each link, respectively. All of the parameters in
the equation relate to each other since they are derived inrelative polar coordinates and in the dimension of torque.
Therefore, most parts of the equations must be revisedwhen the structural configuration of the link system ischanged.

2.2 Handling of Generated Forces in Nodal Force
VectorOn the other hand, torque values are calculated by using
the following equation in the parallel solution scheme[4,5];
fún g = [Ln ][T n ]fP n g ,(3)where [M ] is the total mass matrix, fF g the external force
vector, fRg the internal force vector, fÅ ug theincremental displacement vector, fu_ g the velocity vector,
ug the acceleration vector. β is the integral
and f°
parameter for Newmark’s β method [6], a widely used
time integration scheme. The operation distance between
each incremental step calculated from a target trajectory isused as an input for fÅ ug . The velocity and acceleration
vectors can also be given directly as input data, but weused Newmark’s β method ( δ =1/2, β =1/4) for
calculating these values in this paper. The values of theinternal force vector fRg will all actually become zero,
since the consideration of the deformation of linkmembers in the inverse dynamics calculation process is
not required. The external vector includes informationsuch as dead loads and other additional forces acting at
nodal points. Each corresponding term in the incremental
nodal force vector is then substituted into Eq. (2) tonconstitute the vector fP g . Although the scheme requires
incremental calculation, it has been confirmed in previous
works [4,5] that the calculation time in an actual control issuppressed to a practical value.

Dynamic equations for link systems are generally derived
by the Newton-Euler method or the Lagrangian method.
In a summarized expression, the equations are written as°g + fV (í; í_ )g + fG(í)g
fúg = [M (í)]fí
,

1
1
1
fug
_ tÄ(
Ä 1)f°ugt )fÅug Ä
,
åÅt2
åÅt
2å

If the control torque for generated forces is required inaddition to the dynamic compensation, the torque values
should be added, in the conventional schemes, to the righthand side of Eq. (1) as follows:(2)_)g + fG(í)g + [J ]T fFG g (4)°g + fV (í; ífúg = [M (í)]fí,

where fún g is the torque vector, fP n g the vector related
to nodal forces, [T n ] the transformation matrix between
global and elemental coordinates, and [Ln ] the member
length matrix. The superscript on the upper right indicates
the total number of links. The nodal forces are evaluatedin an absolute Cartesian coordinate system, and in thedimension of force. The equation is completely separated
into terms of different parameters, and each matrix has aclear physical meaning related to the modeled link system.
The member length matrix [Ln ] , for example, contains
components in the dimension of length, which play roles
in converting force to torque. Also, the configuration of
the components in the matrix expresses the structuralconnectivity of the link system. The separation of theparameters makes the equation highly expansible andflexible, and thus the scheme becomes applicable tocomplex link systems without difficulty.

where [J ] is the Jacobian matrix, and fFG g is thegenerated force vector. The Jacobian matrix is derived
from kinematic equations, and we need to do summationof these two different sets of torque in this case.
On the other hand, the control torque for generated forces
can be easily considered in the parallel solution scheme,by simply adding the generated force values to the vector
related to nodal forces fP n g , in Eq. (2). The vector gives
information of the nodal forces acting on the constituting
link members (see Fig. 1), and is defined as

An incremental nodal force vector fÅ f g , required for thelink system in motion between time t and t + Åt , is

240

9
8
FiX
>
>
>
>
>
>
>
>
>
>
>
>
F
iY
>
>
>
>
8
9
>
>
>
>
>
P1 >
>
FiZ
>
>
>
>
>
>
>
>
>
P
>
>
>
>
n
>
>
>
>
>
>
>
P
F
=
< Pj=i+1 jX >
< 2 =
n
n
fP g =
Å ; where fPi g =
F
jY
>
>
>
>
Pj=i+1
>
>
>
>
n
>
>
>
>
>
>
FjZ >
Å >
>
>
>
>
j=i+1
>
>
>
>
>
>
:
;
>
>
>
>
Pn
F
iûX
>
>
>
>
>
>
>
>
>
>
FiûY
>
>
>
>
;
:
FiûZ
.

RollerZ

θ２ｚ
FG
Joint2

Link2
θ１
Link1

Joint1XXθ １, θ ２ ： Rotational angle of Joints 1 and 2(5)

F G： Generated Force

Fig. 2 A wall-trace task for a 2-link manipulator

Here, the subscripts i and j indicate the link numbers (i, j= 1~n), X, Y, Z the global coordinates, and φ therotational components, respectively. The 1st~3rd row
components in fPi g are the nodal forces acting at thecenter of gravity, 4th~6th the sum of nodal forces acting at
the tip of the i-th link member, and 7th~9th the moment ofinertia acting around the center of gravity. The generated
force values are added to each component in fPi g , which
can be specified in input data. Namely, there is norequirement for additional calculation of the Jacobianmatrix or revision of the equation, etc., in spite of theconsideration of the completely different types of torquevalues.

Torque[Nm]
0.15

0.1

0.05

0

Proposed scheme
Inverse Dynamics[J]T{FG}
Conventional scheme

2.3 Numerical Example
0

A simple numerical test is carried out to verify thevalidity of the parallel solution scheme. A wall-trace task
is given to a 2-link manipulator as shown in Fig. 2. A
force value of 0.4 N is constantly generated at the tip ofthe manipulator against the wall. Figure 3 shows the joint
torque curves of Joint 1, which are calculated for the task.
The torque curve obtained from the parallel solution
scheme is in perfect agreement with that from the
Pn

Pn

FiûZ
Link i

FiûY

Z

0.6

0.8

1

T[s]

3. Control Experiments
The parallel solution scheme is implemented into a force
control system as shown in Fig. 4, in which the PID
feedback, force feedback and feed-forward control are all
combined in the design. A target trajectory based upon the
Cartesian coordinates is given as an input, and control

Pn

Link i + 1
j=i+1 FjX

y

FiX

FiûX

0.4

conventional scheme, which is the sum of inverse
dynamics and the control torque for the generated force.

j=i+1 FjY

FiY

0.2

Fig. 3 Joint torque curves for the wall-trace task

j=i+1 FjZ

FiZ

FG

Z

z

τ I .DPC
X , Z , qd , FG

Correction
considering
friction

Calc. of inverse
dynamics byparallel solution sch.

Y

x

FG

Elemental coordinates

KF+

q& d , q d

F feedback

PID

FS

Global coordinates

Controlfunction& qq,
AD Board

X, Z, Qd : Coordinates in global Cartesian coordinates
FG , FS：Nodal point: Target force, actual force

qd , q_d : Target angle and angular velocity

q , q_ : Actual angle and angular velocity：Nodal point expressing center of gravity

KF

Fig. 1 Nodal forces acting on i -th link in an openloop n-link mechanism: Feedback gain for force

Fig. 4 Outline of the force control system

241

Control
deviceτfeedback

－

X

LINK

τ

Encoder
Force sensor

Object

Joint1
Joint2
Joint3
Joint4

Rotational angle[rad]

0.5

2

4

Joint 1

3

0

-0.5

-1

-1.5

Fig. 5(a) Two manipulators holding an object

④

②

Load

③

①

Active Joint① Joint No.

0

5

10

T[s]

Fig. 6 Target motion for two manipulators inforce control

Pin joint

approximately 0.7 ms by using a Celeron 1.1 GHz PC,
which is short enough for practical use, when thesampling time is set to 10 ms. By adding frictionalcorrection in motors and feedback values to τ I.D., thefinal control torque τ is obtained as follows:ú= J q°d + Dq_d + fc + úI:D: + úf eedback ,Force sensor

where J is the moment of inertia of motors, D theviscosity coefficient of motors, fc the dynamic friction
force, and q_d and q°
d the target angular velocity andangular acceleration, respectively. τ feedback is the PID
feedback torque, which is obtained using

Fig. 5(b) Hand device

torque values including the effects of generated forces arecalculated successively in the PC as τ I.D.. The main
purpose of developing the system is not to create anotherconcept of a force controller, but simply to verify theaccuracy of calculated outputs, and to check the
performance of the calculation itself in actual control
process. The force feedback value, which is obtained byusingúf eedback = KP (qd Ä q) + KI Ü(qd Ä q) + KD (q_d Ä q)
_ , (8)where q and q_ are the actual angle and angular velocity
acquired from the attached encoders, respectively. KP, KI
and KD are the feedback gain for the angle, the integrated
value and the angular velocity, respectively.
Figure 5 shows a set of two manipulators used in theexperiment, with hand devices to hold an object. Gearless
motors are adopted, in this paper, to deliberately
maximize the effect of dynamics and to verify the outputof the parallel solution scheme. Feedback gain values
used in the experiment are shown in Table 1. The
manipulators are given a task to move a woodchip(weight: 15 g) along a circle path for two times, with aconstant grasping force of 0.5 N. Figure 6 shows therotational angle for each joint and the schematic diagramof the target motion. Torque curves for each joint are
obtained during the control by using the parallel solution
scheme as shown in Fig. 7. The value of inverse dynamics
is considerably large compared to the total torque asshown in Fig. 8, and thus, it is desirable not to disregard
the calculation of inverse dynamics in this kind of motion,
to maintain the stability of the control. Figures 9 and 10
show the control results of the rotational angle and thegenerated force. It is evident that the control results of(6)

Ff eedback = KF (FG Ä FS ) ,is directly fed back into the calculation. Here, KF is the
feedback gain for force, FG the target generating force andFS the actual value acquired from the force sensors. The
calculation is carried out incrementally in each samplingtime period. The torque values on all joints are obtained
after the calculation is finished for each time period. For
example, it is confirmed that the calculation takes
Table 1 Feedback gain values

K P Nm/rad

J1
4.5

J2
3.5

J3
4.5

J4
2.5

K D Nms/rad

1.8

1.5

1.8

1.0

K I Nm/rad

0.04

0.06

0.02

0.04

KF

(7)

0.00018242

Torque[Nm]0Rotational angle[rad]

-0.02
-0.04
-0.06
-0.08

Joint1

0-0.5
Joint2

Joint1
Joint2

-0.1
-0.12

0.5

0

5

10

(a) Joints 1 & 2-1

TargetControl result
0

T[s]

5

10

T[s]

(a) Joints 1 & 2Torque[Nm]Joint4
Rotational angle[rad]

0.08
0.06
0.04
0.02

0.5

0

-0.5

Joint30
Joint3
Joint4

-0.02
0

5

-1

10

0

T[s]

(b) Joints 3 & 4

TargetControl result
5

10

T[s]

(b) Joints 3 & 4

Fig. 7 Torque curves for two manipulators inforce control

Fig. 9 Control results of motion

Torque[Nm]

F[N]
0.75

0

0.5

Joint 1
-0.05

0.25
-0.1

TargetControl result

Total torque
Inverse dynamics
0

5

10

0
T[s]

0

5

10

T[s]

Fig. 10 Control result of generated force

Fig. 8 Comparison of inverse dynamics against
total torquepractically maintained to the target. These results confirm
the validity of the torque values calculated by the parallel
solution scheme.

motion are in good agreement with the target. Although
the control result of generated force contains high-mode
oscillation that is assumed to be a white noise detected by
the force sensor, the generated force is consistently and243

4. Conclusion

References:

The formerly proposed parallel solution scheme is applied
to force control of link systems, where a consideration ofboth generated forces and dynamical effects is inevitable.
The numerical and experimental results show that theaccurate values of control torque considering both effects
are obtained by using the parallel solution scheme. The
scheme does not require derivation of Jacobian matrices
and revision of dynamic equations even in such cases, andthe outside frame of the numerical algorithm remains thesame. The scheme can be applied to complex systems
where calculations of kinematic and dynamic equations
are essentially difficult.

[1] Y. Nakamura, Dynamics computation of closed linkrobots and optimization of actuational redundancy, Trans.
Society of Instrument and Control Engineers, 25(5), 1989,
600–607, in Japanese.
[2] K. Sugimoto, Deviation of equation of motion ofclosed loop mechanisms, J. Robotics Society of Japan,
15(3), 1997, 460–467, in Japanese.
[3] Y. Nakamura and K. Yamane, Dynamics computationof structure-varying kinematic chains and its application
to human figures, IEEE Trans. Robotics and Automation
16(2), 2000, 124-134.

The vector related to nodal forces, on which we focusedin this paper, can contain values with both static anddynamic effects. Studies on inverse dynamics calculationand feed-forward control of flexible manipulators, which
can be developed by using this feature, are in progress.

[4] D. Isobe, Y. Chikugo, D. Imaizumi, S. Sato, and A. Yagi,
Feed-forward control of link mechanisms under various
boundary conditions by using a parallel solution scheme, Proc.
2003 IEEE/RSJ Int. Conf. Intelligent Robots and Systems
IROS2003, Las Vegas, 2003, 2773-2778.
[5] D. Isobe and D. Imaizumi, A parallel solution schemeof inverse dynamics for flexible manipulators, Proc.
IASTED International Conference on Robotics andApplications (RA2003), Salzburg, 2003, 106-111.

5. Acknowledgement
The authors would like to extend their appreciation for thecontributions of the former members of their laboratory,
Takeo Ueda, Daisaku Imaizumi, Youichi Chikugo andAtsushi Yagi.

[6] N.M. Newmark, A method of computation forstructural dynamics, A.S.C.E. J. Engineering Mechanics,
85, 1959, 67-94.

244