SICE Annual Conference 2008
August 20-22, 2008, The University Electro-Communications, Japan

Novel Technique of Contact Force Vector Determination Aimed at Control of Service Robot
Arm and Estimation of Environment Stiffness
Dzmitry Tsetserukou, Naoki Kawakami, and Susumu Tachi
Department of Information Physics and Computing, University of Tokyo, Tokyo, Japan(Tel : +81-3-5841-6917; E-mail: dima_teterukov@ipc.i.u-tokyo.ac.jp)
Abstract: In this paper, we focus on the approach to calculation of the applied force vector at any point of the robot arm
and its technical realization. The experimental results validate the ability of the proposed method to trace the contact
point position and to estimate force vector accurately. The technique of calculation of contacting object stiffness duringcollision is elaborated and experimentally examined. The algorithm of contact transition control considering theenvironment stiffness value and its application for tactile interaction with surroundings are discussed.
Keywords: Sensitive robot arm, robot-environment interaction, contact transition control.

1. INTRODUCTION

Yaw

In the past decade, many researches have studied how
to avoid unexpected collision of robot with environmentor human beings. Several effective collision avoidance
strategies were developed. Lumelsky et al. [1] proposed
to cover manipulator with optical proximity sensors
capable of detecting nearby objects. The high-speed
vision system attached to the robot arm allows real-timecollision avoidance [2]. However, even such complex
and skilled sensory system as human beings have cannot
avert collisions in our daily lifetime and we must
presume such situation during robot operation.
Furthermore, such tasks under human supervision astransporting the object, leading the robot tip via
force-following, performing the assembling tasks,
require the processing algorithm of contact state.
Instead of the general concept of collision avoidanceby means of deliberate shunning of contact, we propose
the concept of collision avoidance through interaction.
The main advantage here is that during contact we canacquire additional information about the object, namely,
its stiffness, texture, shape, and relative position.
Conventional approaches to controlling the physicalinteraction between a manipulator and environment are
based on impedance control of a robot arm according toapplied force vector measured at the manipulator wrist[3]. However, the rest parts of the robot body (forearm,elbow, upper arm, shoulder, and torso) are presenting
significant danger not only for human being, but also forthe robot structure itself.
Distributed optical joint torque sensors and local
admittance controllers endow our robot arm with thedistinctive capability of safe interaction withsurroundings along entire manipulator surface. The
developed robot arm is shown in Fig. 1(a). The
coordinate systems fixed to the joints and connected tothe arbitrary contact points are given in Fig. 1(b).
The paper focuses on approach to determination of
the applied force vector at each point of the robot arm.
This information is essential to plan collision avoidance
trajectory, structure the environment, and communicate
with humans intelligently. The proposed method takes

X2
Z2

Opticaltorquesensors
Tactilesensors

J2

x2
F2
Z4

Y4

X4

z2

Y3

a)

Y0
+
X0 +
Pitch
Roll

Y2

J1

Z1
Y1

L F2

X1

L2

y2
J4

X3
z4

Z5
X5

Z0

J3

F4

Y5

+

Z3
y4

L F4
L4

x4

b)

Fig. 1 Robot arm and frame assignments.
advantages of the tactile sensor, enabling to detect
coordinates of the contact point, and the distributed joint
torque sensors to measure exerted torques and force
vector direction on the entire robot arm surface(including joints). This method originates from humantactile system. Very fine special resolution of the humanskin allows performing tactile discrimination of surface
texture by fingertips. However, it has moderate ability toestimate the exerted force, the job that muscles do. They
generate torque at the skeletal joints balancing suchexternal forces as gravity, mechanical constraints ofjoints, inertial forces, and contact forces. Each muscle
produces a torque at a joint that is the product of its
contractile force and its moment arm at that joint [4].
Hence, by estimation of the tension of thecorresponding muscles, we can define the acting force
as well as the force direction.
In the paper we also address contact transition control
which enables estimation of contacting object stiffness
and trajectory following targeted by object surface.
The remainder of the paper is structured as follows.
Section 2 introduces the approach to calculation ofcontact force vector. The robot sensory system andexperimental results of measurement of contact force
vector are presented in Section 3. The methods for- 2771 -PR0001/08/0000-2771 ¥400 © 2008 SICE

3. ROBOT SENSORY SYSTEM AND
EXPERIMENTAL RESULTS

contact state recognition and estimation of contacting
object stiffness are detailed in Section 4. In Section 5,
we conclude the paper and discuss the future work.

2. CALCULATION OF CONTACT FORCEVECTOR
The proposed algorithm of computing the force
vector at any contact point on entire teleoperated robot
arm employs information about the contact point
coordinates and joint torque values.
The forces ifi acting in the coordinate system of eachjoint produce moments ini (Eq. (1)). The joint torques
are derived by taking Z component of the moments
applied to the link (Eq. (3)).
i

ni

i
i 1

i

i
i 1

fi

Ĳi

R i 1ni 1  i PCi u i Fi  i Pi 1 u i 1i R i 1 f i 1

(2)

R i 1 fi 1  i Fii T i ˆ
n Z ,
i

(1)
(3)

i

where i+1iR is the matrix of rotation between links i+1
and i calculated using Denavit-Hartenberg notation;i
Pi+1, iPCi are the vectors locating the origin of thecoordinate system i+1 in the system i, and contact point,respectively; iFi is the contact force.
Let us consider the case when force is applied to theupper arm of the robot. Using the Eq. (1), (2) we have:n2ª 0 º ª F2 x º«L » u «F »« F2 » « 2y »«¬ 0 »¼ «¬ F2 z »¼

n1

ª s2
«
«0
«¬c2

2

1

c2
0
 s2

ª LF 2 F2 z º« 0 »;«»«¬  LF 2 F2 x »¼

0 º ª LF 2 F2 z º
1 »» «« 0 »»0 »¼ «¬  LF 2 F2 x »¼

ª LF 2 F2 z s2 º
« L F » .
« F 2 2x »«¬ LF 2 F2 z c2 »¼(4)

The components of the applied force vector and force
magnitude are derived from:F2 x

Ĳ 2  LF 2 ; F2 z

F2

F2 x

2

 F2 z2

Ĳ1 LF 2 c2 ;.

(5)

The value of the applied force projection on the axis Y
is not required because the upper arm cannot move inthis direction. For the case when the force acts on theforearm we have the following algorithm to obtain thecontact force vector:
F4 y
F4 x

3.1 Sensory system
For practical implementation of the proposed
approach we developed sensory system of the robot arm
which includes tactile skin and optical torque sensors
distributed into each joint.
The robot arm is covered with Kinotex tactile sensor
measuring the pressure intensity through amount ofbackscattered light by photodetector [5]. The sensitivity,
resolution, and dynamic range of this artificial skin arecomparable to those of a human. The taxels displacedwith 21.5 mm in X and 22 mm in Y direction make up
6x10 array. The developed program code analyses thetactile pattern and calculates the number of contacts andthe center of gravity of the contact pattern based on thepressure distribution information.
In technical specifications of the Kinotex sensor, theability of force measurement is indicated. However, thefollowing shortcomings complicate the accurate
measurement of the applied force: temperaturedependency of the transducer constant, fragility whilemechanical overloading, large hysteresivity, highnon-linearity of the output, limited sensing range, etc.
Additionally, the tactile sensors detect and measure thespatial distribution of forces perpendicular to apredetermined sensory area and, hence, cannot evaluate
the vector of applied force. Thus, the most appropriate
application of such tactile sensors is the contact area
recognition and the measurement of relative pressure
distribution in the contact region on the robot bodysurface. The task of load measurement is accomplishedby the developed optical torque sensors enabling torquemeasurement with high accuracy.
In order to recognize the contact region, we employed
the watershed algorithm, an image processing
segmentation technique that splits an image into areas
based on the topology of the image [6]. The intuitive
idea underlying the approach comes from the field of
topography: a drop of water falling on the relief follows
a descending path and ultimately reaches a minimum.
The adopted algorithm is processed as follows:
departing from the local maximum, the new taxel is
included into the contact area only if within the 3x3
array surrounding it there is an other taxel of greater or
equal pressure intensity already included into the water
stream.
The accurate estimation of the contact point can beobtained by computing the center of gravity of thecontact pattern c(xc,yc) of the neighborhood : by:

Ĳ 2  F4 z ( LF 4 c3c4  L2 c3 )  F4 x ( LF 4 s3  L2 s3c4 )L2 s3 s4
Ĳ 4 LF 4 ; F4 z

Ĳ3 LF 4 s4 ,c xc , yc(6)where c2, c3, c4, s3, and s4 are abbreviations for cos(T2),
cos(T3), cos(T4), sin(T3), and sin(T4), respectively; L2,
LF2, and LF4 are the lengths of the moment arms.

§ ¦ xi f i xi , yi ¦ yi f i xi , yi ·¨ x , y:¸,
x , y:,¨
f i xi , yi
f i xi , yi ¸¸¦¨ x¦
x , y:© , y:¹(7)

where fi(xi,yi) is the pressure intensity level of the taxel iwith coordinates (xi,yi).

- 2772 -The example of contact area image by using Kinotex
skin is presented in Fig. 2.
0

0

0

0

0

0

0

15

0

0
0

0

0

4

7

0

0

0

0

114

190

0

0

0

26

1303 754

4

0

0

59

859

463

0

0

0

553

796

0

0

0

0

0

0

12

0

0

0

0

0

0

0

0

0

0

0

0

0

0

2.0

yc

Applied torque WEXT, Nm

0Fig. 4(c). While contacting, the torque values measured
at the first and second joint (W1, W2) vary in accordancewith Fig. 4(a). The contact force vector projections (F2x,F2z) and force vector length F2 obtained from Eq. (5) arerepresented in Fig. 4(b). The trajectory of the end pointof the contact force vector is shown in Fig. 4(d).

xca) Digital image

1.5
1.0
0.5
0.0

-0.5

b) Visual image

Fig. 2 Representations of the contact area.

-1.5

In order to facilitate the realization of torquemeasurement in each arm joint, we developed new
optical torque sensors based on results presented in [7].
The novelty of our method is application of theultra-small size photointerrupter (PI) RPI-121 assensitive element to measure relative motion of sensor
components. The dimensions of the PI (3.6 mm u 2.6
mm u 3.3 mm) and its weight of 0.05 g allow realizationof compact design. The optical torque sensor is set
between the driving shaft of the harmonic transmission
and driven shaft of the joint (Fig. 3). When the load isapplied to the robot joint, the magnitude of the output
signal from the PI corresponds to the exerted load.

12

Contact force components F2, N

a) 0

0

4

Fig. 3 Torque ҏsensor of the elbow joint.
The spring members attached to the first, second, andthird/fourth joints were designed to measure torque of r
12.5 Nm, r 10.5 Nm, r 4.5 Nm, and have resolution of
10.77 mNm, 9.02 mNm, 4.31 mNm, respectively. Eachsensor was calibrated by means of attachment ofreference weights to the lever arm. Non-linearity of
2.5 % of Full Scale was calculated using maximum
deviated value from the best-fit line.
The developed optical torque sensors have highdependability, good accuracy (even in electrically noisy
environment), low price, compact sizes, light weight,
and easy manufacturing procedure.
3.2 Experimental results
To evaluate the feasibility of the proposed approach,the experiments were conducted for calculation of thecontact force vector acting on the upper arm.
During the experiment the object was moved along
the upper arm surface according to the curve shown in

12

16Time t, sec

200
180
160
140
120
100
80

Harmonic
drive

8

F2
F2z
F2x

-8

Contact force component Fz2 , N

Spring

16

-4

120

c)
W T1

12

0

-12

T2

8

Time t, sec

4

b)

PI

4

8

Contact point location y, mm

ShieldApplied torque W
Applied torque W

-1.0

140

160

180

200

220

-6

-4

-2

0

Contact point location x, mm

8
6
4
2
0
-2
-4
-6
-8
-12

d)

-10

-8

Contact force component Fx2 , N

Fig. 4 Experimental results.
Robot sensory system and proposed algorithm havedemonstrated ability of contact force vector calculation.

4. ROBOT-ENVIRONMENT INTERACTION
4.1 Contact state recognition
During the first stage of control the robot links rotate
until one of them contacts the object. First, thealgorithm of contact detection must be elaborated.
In addition to contact force, torque sensors
continuously measure the gravity and inertial load. As- 2773 -robot arm moves with low angular speed, the inertial
load component can be disregarded. Let us consider
gravity torque calculation in the case when robot arm
performs only planar motion, and only the first andfourth joints operate (Fig. 5).

Wg1

LM 2
LM 4
Wg4

m2 g

T4

m1 g

Fig. 5 Robot arm subjected to gravity loading.
The Newton-Euler dynamics formulation was
adopted to calculate reference value of the gravity
torques. The gravity torques acting in the first Wg1 andfourth joints Wg4 are derived from:
Ĳ g1

m2 g LM 4 sin(ș1  ș 4 )  L2 sin(ș1 )  m1 gLM 2 sin(ș1 )Ĳg 4

m2 gLM 4 sin(ș1  ș 4 ) ,4.2 Estimation of contacting object stiffness
During contact transition we can acquire information
about collision danger of contacting object through its
stiffness estimation. This can be done by establishing
stiff contact through PD control of robot arm with highP-gain and setting the high threshold value. For thefollowing experimental results the threshold was chosen
as high as 0.05 Nm. The robot was commanded tofollow the trajectory in free space with constant angularvelocity. An object was placed on this path so that thesecond link will contact the object. The joint torque wasrecorded for the fourth joint while contacting with
object. The Figs. 7(a)~7(e) show experimental results
when link comes into contact with objects having
different stiffness varying from very low rate to very
high, namely, piece of sponge, rubber sponge, rubber,chemical wood, and aluminum, respectively. The time
derivative of torque during impact is given in Fig. 7(f).

(8)

0.05

Joint torque W4, Nm

T1

inertial loading that provokes the vibrations during
acceleration and deceleration transient. This disturbancecan be evaluated by using accelerometers and excluded
from further consideration. Observing the measurement
error plot, we can assign the relevant threshold thattriggers control of constraint motion.

T4 = 0.4184 deg
W4 = 0.05 Nm

0.04
0.03

0.300

0.00
0.0

a)

Joint torque W4, Nm

60

90

120

Joint angle T4 , deg

180

0.03

Upper margin of average errore)Maximum error

0

30

60

90

120

Joint angle T4 , deg

150

0.14

T4 = 0.0237 deg
W4 = 0.0208 Nm

0.042

0.056 .00

d)

0.01

0.02

0.03

Joint angle T4 , deg

0.20
0.15

Force arm length PC4 = 0.175 m

0.00
0.000

Lower margin of average error

0.105

Force arm length PC4 = 0.1755 m

T4 = 0.0334 deg
W4 = 0.049 Nm

0.04

Pick value
Time derivative of torque, Nm/s

0.10
0.05

T4 = 0.0176 deg
W4 = 0.0197 Nm

0.01

-0.02

b)

0.028

0.02

0.00

0.070

Joint angle T4 , deg

T4 = 0.0457 deg
W4 = 0.05 Nm

Joint angle T4 , deg

0.04

Maximum error

0.02

0.014

0.05

150

0.035

T4 = 0.0344 deg
W4 = 0.0211 Nmc)Joint torque W4, Nm

Error of measurement H, Nm

30

b)

Force arm length PC4 = 0.17 m

0.00
0.000

0

0.4 .000

T4 = 0.058 deg
W4 = 0.049 Nm

0.04

Measured torque Wm
Reference torque Wr

0.04

-0.04

0.3

0.01

0.000

a)

0.2

T4 = 0.0716 deg
W4 = 0.02 Nm

Joint angle T4 , deg

0.05

0.075

-0.005

0.1

0.02

0.150

Force arm length PC4 = 0.17 m

T4 = 0.1919 deg
W4 = 0.0202 Nm

0.01

0.03

0.225

Force arm length PC4 = 0.17 m

0.02

Joint torque W4, Nm

where m1 and m2 are the point masses of the first andsecond link, respectively; LM2 and LM4 are the distancesfrom the first and second link origins to the centers of
mass, respectively.
The experiment with the fourth joint of the robot arm
was conducted in order to measure the gravity torque(Fig. 6(a)) and to estimate the error by comparison withreference model (Fig. 6(b)).

T4 = 0.1336 deg
W4 = 0.05 Nm

0.00
-0.05

0.008

0.016

0.024

Joint angle T4 , deg

0.032 0.0f)

-5

5.0x10-4

1.0x10-4

1.5x10Time t , sec-4

2.0x10Fig. 7 Experimental results of stiffness estimation.

180

Fig. 6 Experimental results of gravity measurement.
As can be seen from Fig. 6, the pick values of thegravity torque estimation error arise at the start and stop
stages of the joint rotation. The reason of this is highIt is apparent from the above presented plots that the
stiffer object comes into contact with robot arm thesmaller angle the joint rotates. Heterogeneous nature ofsponge material explains highly non-linear behavior of
stiffness curve (Fig. 7(a)).

- 2774 -The elastic deformation of the object and inherent
compliance of the robot joints lead to rotation of robot
arm of angle 'Ti during contact transience (Fig. 8).

The total elastic deformation Gi is made up of elastic
deformation caused by object compliance Go and one
generated by joint flexibility Gri. That is, we can write:
įi

Fni
Wi
O

Mi

ri
'Ti

hi

A

PCi
ri

Fni
Gi

C

bri
bo Rni

kri

Mi

ko
C'

B

1

1'

Fig. 8 The scheme for elastic deformation calculation.
The distance, which contact point C on the robot link
surface 1 moves perpendicularly to the radius ri under
torque 'Wi, is equal to ri'Ti (since the angle 'Ti is fairly
small). The unknown angle Mi can be found by notingthat C’CB=OCA=Mi:ĳi§ hi ·§ OA ·
arctan ¨¸,¸ arctan ¨¨¸P© CA ¹C© i¹(9)where hi equals half of the robot link thickness (forforearm h = 0.03 m).
From the right triangle 'CBC’ the total elastic
deformation is calculated as:įi'și ri cos ĳi .

(10)

kri

ko

=

'Ĳi
PCi 'și PC2i  h 2 cos arctan(hi PCi )

.

(11)

By linear approximation, we take into account only
two values of Ti for Wi nearest to 0.02 Nm and 0.05 Nm.
Then, from the experimental diagrams (Figs. 7(a)~7(e))
we obtaine 'Ti and 'Wi listed in Table 1.
Table 1 Findings from experimental diagrams.
RubberParameter
SpongeRubber Woodsponge'Ti 10-4 [rad]

39.53

10.82

4.12

3.84

'Wi 10-2[Nm]

2.98

3.0

2.79

2.92

0.0293 Nm
N=3468.6
0.175  2.758 104  0.1776  0.986 m 2m

kri  ki ,kri  ki

(13)

where ki is defined by Eq. (11). Derived values of ki andko are given in Table 2.
Table 2 Total stiffness and object stiffness.
RubberParameter Sponge
RubberWoodspongei'Ĳi=
PCi 'și ri cos ĳi(12)

The stiffness of the objects made from sponge, rubbersponge, rubber, and chemical wood are calculated fromthe following equation:

Now, we can easily solve for total stiffness:FniįiRni Fni ,ko kriwhere Rni is the reaction, absolute value of which equals
Fni; ko and kri are stiffness of the object and stiffness ofrobot link, respectively.
The coefficient kri is mainly defined by torque sensor
stiffness, harmonic drive stiffness, structural flexibility,
and P-gain magnitude. Detailed examination has
showed that complex theoretical model of robot link
stiffness can hardly provide accurate estimation of kri.
Rather, we can set the value of robot link stiffness close
to total stiffness in the most hard contact case. This
assumption is valid because during impact with hardenvironment, such as aluminum plate, the contact
deformation of the object is too small to be accounted(ko | f). Thus, from the data listed in Fig. 7(e) andusing Eqs. (9) ~ (11) we derive unknown value of kri:Radius of contact point trajectory ri is found throughconsideration of right triangle 'CAO as ri
PC2  hi2 .

kiFni=įo +į rikiki [N/m]

260.84959.3

2343.782469.04

ko [N/m]

282.041325.927223.948562.90The
obtained
results
demonstratestrongcorrespondence of correlation among calculated object
stiffness with that of real objects. Naturally, the actual
stiffness of colliding environment differs from
calculated one with finite error. To achieve highaccuracy, specific equipment is needed. However, ouraim was only assessment of the danger level of robot
arm collision with object. Specifically, we can definethat sponge and rubber sponge material are safe forinteraction but rubber, wood, and metal pose threat
while striking the robot arm. Consequently, we havesucceeded in solving main task, that is, object
classification by getting information about stiffness
during impact.
In case when collision must be robustly detectedwithout consideration of object property, the value of- 2775 -time derivative of torque can be used to judge theimpact value (Fig. 7(f)).
4.3 Control of physical interaction of robot arm withsurroundings
As soon as the joint torque value exceeds thethreshold, the control is transmitted to local admittancecontroller. Thus, on the last stage of contact transience
compliant interaction is provided with predetermined
dynamics. The desired impedance properties of i-th jointof manipulator can be expressed as:
J di 'și  Ddi 'ș i  K di 'și

Ĳ EXTi ; 'ș iș ci  ș di ,(14)

where Jdi, Ddi, Kdi are the desired inertia, damping, andstiffness of i-th joint, respectively; WEXTi is torque applied
to i-th joint and caused by external and gravity forces,'Ti is the difference between the current position Tci anddesired one Tdi.
The remarkable opportunity is that we can establish
different dynamic parameters of robot arm interaction
with environment according to its stiffness. This isinherent capability of humans, when we collide withstiff objects we tend to soften our muscles as we can.
Another possible application of proposed approach is
to structure the environment. At present time, visionsystem of robots cannot correctly determine the distanceto the object in 3D space. Thus, such simple task as
taking a cup from table by hand is extremely difficult
for robots. By contact interaction, we can definereference coordinate system and reachable working area.
As example of such control implementation, the tablesurface following trajectory and force vector direction ateach motion step are shown in Fig. 9.

Fni
L2
L4

Fig. 9 The table surface following trajectory.
The robot control through tactile interaction would
endow the robot with human-like ability to safely
interact with environment.

5. CONCLUSIONS AND FUTURE WORK
It was shown experimentally that the developed
algorithm and implemented sensory system of the robot
arm allow measuring the force direction at any point of
the robot arm accurately and robustly.
The main stages of contact transition control were
described. Then, we detailed the procedure of contact
state recognition. As soon as contact with object isestablished, the robot arm changes own dynamic
parameters according to estimated object stiffness. The
presented method of object deformation calculation
allowed estimating the level of collision dangerousness.
Analysis of experimental results elicited a fact of strong
correspondence of correlation among calculated object
stiffness with that of real objects.
In the future research we are going to employ the
information on force vector and contact point to plan thecompliant collision avoidance trajectory autonomously
and provide tactile feedback to the operator. We
consider that presenting the information about direction
and intensity of the contact force vector can greatly
improve the collision avoidance strategy realized byoperator, in comparison with the conventional contact
area tactile feedback. On the next stage we will also
realize presented in the paper idea of trajectory
following targeted by object surface.
Acknowledgments. The research is supported in partby a Japan Society for the Promotion of Science (JSPS)
Postdoctoral Fellowship for Foreign Scholars.

REFERENCES[1] V. J. Lumelsky and E. Cheung, “Real-time
Collision Avoidance in Teleoperated Whole-sensitive
Robot Arm Manipulators”, IEEE Trans. on Systems,
Man, and Cybernetics, Vol. 23, No. 1, pp. 194-203,
1993.
[2] S. Morikawa, T. Senoo, A. Namiki, M. Ishikawa,“Real-time Collision Avoidance Using a Robot
Manipulator with Light-weight Small High-speed
Vision System” IEEE Int. Conf. Robotics andAutomation, Roma, pp. 794-799, 2007.
[3] F. Caccavael, C. Natale, B. Siciliano, L. Villani,“Six-DOF Impedance Control Based on Angle/Axis
Representation”, IEEE Transaction on Robotics andAutomation, Vol. 15, No. 2, pp. 289-300, 1999.
[4] E. R. Kandel, J. H. Schwartz, T. M. Jessell,
Principles of Neural Science, McGraw-Hill, New York,2000.
[5] Optic Fiber Tactile Sensor Kinotex, Nitta
Corporation, “http://www.nitta.co.jp/”[6] J. Serra, Image Analysis and Mathematical
Morphology, Academic Press, London, 1982.
[7] D. Tsetserukou, R. Tadakuma, H. Kajimoto, S.
Tachi, “Optical Torque Sensors for Implementation of
Local Impedance Control of the Arm of Humanoid
Robot”, IEEE Int. Conf. on Robotics and Automation,
Orlando, pp. 1674-1679, 2006.

- 2776 -