Robot Force Control

ROBOT FORCE CONTROL

BRUNO SICILIANO AND LUIGI VILLANI

PRISMA Lab
Dipartimento di Informatica e Sistemistica
Università degli Studi di Napoli Federico II
Via Claudio 21, 80125 Napoli, Italy

Kluwer Academic Publishers

Boston/Dordrecht/London
To C. A. & F.
—BS

To My Parents
—LV
Contents

Preface xi
Acknowledgments xiii
1. INTRODUCTION 1
1. Motion control vs. interaction control 1
2. Indirect vs. direct force control 2
3. Experimental apparatus 4
2. MOTION CONTROL 7
1. Modeling 7
1.1 Kinematics 8
1.2 Dynamics 10
2. Tracking control 12
2.1 Dynamic model-based compensation 12
2.2 Euler angles error 14
2.3 Angle/axis error 16
2.4 Quaternion error 19
2.5 Computational issues 21
2.6 Redundancy resolution 23
3. Regulation 26
3.1 Static model-based compensation 26
3.2 Orientation errors 27
4. Further reading 28
3. INDIRECT FORCE CONTROL 31
1. Compliance control 31
1.1 Active compliance 32
1.2 Experiments 34
2. Impedance control 36
2.1 Active impedance 36
2.2 Inner motion control 38
2.3 Three-DOF impedance control 40
2.4 Experiments 40
vii
viii ROBOT FORCE CONTROL

3. Six-DOF impedance control 43

3.1 Euler angles displacement 44
3.2 Angle/axis displacement 46
3.3 Quaternion displacement 50
3.4 Experiments 51
3.5 Nondiagonal six-DOF stiffness 61
4. Further reading 63
4. DIRECT FORCE CONTROL 65
1. Force regulation 65
1.1 Static model-based compensation 66
1.2 Dynamic model-based compensation 67
1.3 Experiments 68
2. Force and motion control 69
2.1 Force and position regulation 70
2.2 Force and position control 72
2.3 Moment and orientation control 75
2.4 Experiments 78
3. Force tracking 82
3.1 Contact stiffness adaptation 82
3.2 Experiments 84
4. Further reading 86
5. ADVANCED FORCE AND POSITION CONTROL 89
1. Task space dynamics 89
2. Adaptive control 91
2.1 Regulation 91
2.2 Passivity-based control 97
2.3 Experiments 100
3. Output feedback control 103
3.1 Regulation 104
3.2 Passivity-based control 106
3.3 Experiments 110
4. Further reading 111
Appendices 113
A– Rigid Body Orientation 113
1. Rotation matrix 113
2. Euler angles 114
3. Angle/axis 117
4. Quaternion 118
B– Models of Robot Manipulators 121
1. Kinematic models 121
1.1 Six-joint manipulator 122
1.2 Seven-joint manipulator 127
 Contents ix

2. Dynamic models 128

2.1 Six-joint manipulator 129
2.2 Seven-joint manipulator 131
References 135
Index 145
Preface

Research on robot force control has flourished in the past two decades. Such
a wide interest is motivated by the general desire of providing robotic systems
with enhanced sensory capabilities. Robots using force, touch, distance, visual
feedback are expected to autonomously operate in unstructured environments
other than the typical industrial shop floor.
It should be no surprise that managing the interaction of a robot with the
environment by adopting a purely motion control strategy turns out to be
inadequate. The unavoidable modeling errors and uncertainties may cause a
rise of the contact forces ultimately leading to an unstable behavior during the
interaction. On the other hand, since the early work on telemanipulation, the
use of force feedback was conceived to assist the human operator in the remote
handling of objects with a slave manipulator. More recently, cooperative robot
systems have been developed where two or more manipulators (viz. the fingers
of a dexterous robot hand) are to be controlled so as to limit the exchanged
forces and avoid squeezing of a commonly held object.
The subject of robot force control is not treated in depth in robotics textbooks,
in spite of its crucial importance for practical manipulation tasks. In the few
books addressing this topic, the material is often limited to single-degree-of-
freedom tasks. On the other hand, several results are available in the robotics
literature but no dedicated monograph exists. The book is thus aimed at filling
this gap by providing a theoretical and experimental treatment of robot force
control.
The topics covered are largely inspired by the research work of the authors
and their colleagues in the robotics group at the University of Naples during
the last decade, including the Doctorate thesis on force control by the second
author. Nonetheless, the book is not a mere collection of the results that can
be found in archival publications. The presentation of the various concepts
is rather motivated by the aim to lie the methodology behind the robot force
xi
xii ROBOT FORCE CONTROL

control schemes in a uniform and instrumental yet mathematically rigorous

fashion.
The force control problem is tackled as the natural evolution of the mo-
tion control problem, where feedback of the contact force as provided by a
force/torque sensor is used to manage the interaction of a robot manipulator
with a scarcely structured environment. In this respect, those control strategies,
e.g. hybrid position/force control, devised for interaction with an accurately
modeled environment are not treated.
The contents of the book are organized as follows. The interaction control
problem is discussed in Chapter 1, where the motivation for the material to
follow is sketched. Classical motion control schemes are presented in Chapter 2
with special concern to six-degree-of-freedom tasks and kinematic redundancy.
Compliance control and impedance control are introduced in Chapter 3 as the
basic strategies for indirect force control. Direct force control strategies are
devised in Chapter 4 which are obtained via the closure of an outer force control
loop while motion control is recovered by adopting a parallel composition of
the two control actions. Chapter 5 is devoted to illustrating advanced force
and position control strategies which include passivity-based, adaptive and
output feedback control. Remarkably, most control schemes are experimentally
tested on a setup consisting of a six-joint or a seven-joint industrial robot with
open control architecture and force/torque sensor. Two appendices provide
background material on rigid body orientation and the models of the robot
manipulators used in the experiments.
The book is addressed to scholars and researchers entering the field of robot
force control. The material can be used as a reference for part of a graduate
course on robot control in Electrical and Mechanical Engineering.

Naples, September 1999 Bruno Siciliano and Luigi Villani

Acknowledgments

To Lorenzo Sciavicco goes the authors’ largest sign of recognition for having
pioneered research in robotics at University of Naples. He has ever constituted
an inexhaustible source of inspiration and encouragement to face new chal-
lenges. The authors are grateful to Stefano Chiaverini for his substantial work
in the field and his original contribution of the parallel approach. The material
in Chapter 4 is greatly inspired by the joint papers with him. Warm thanks
are due to Pasquale Chiacchio for having established, with the above two col-
leagues, the PRISMA Lab providing exciting facilities for local research. Most
of the contents of Chapters 2 and 3 are the result of a fruitful cooperation with
Fabrizio Caccavale and Ciro Natale. The latter also deserves a special note of
mention for his invaluable support in the experimental activities, as well as for
useful annotations on a draft of this book. A final word of appreciation is for
Rob Zeller at Kluwer Academic Publishers in Boston for his valuable editorial
assistance.

xiii
Chapter 1

INTRODUCTION

Some introductory remarks on the robot force control problem are given. The rationale
for the control schemes presented in the subsequent chapters is illustrated, together with
a description of the experimental apparatus.

1. MOTION CONTROL VS. INTERACTION CONTROL

Control of interaction between a robot manipulator and the environment is
crucial for successful execution of a number of practical tasks where the robot
end effector has to manipulate an object or perform some operation on a surface.
Typical examples include polishing, deburring, machining or assembly. A
complete classification of possible robot tasks is practically infeasible in view
of the large variety of cases that may occur, nor would such a classification be
really useful to find a general strategy to control interaction with environment.
During interaction, the environment sets constraints on the geometric paths
that can be followed by the end effector. This situation is generally referred
to as constrained motion. In such a case, the use of a purely motion control
strategy for controlling interaction is a candidate to fail, as explained below.
Successful execution of an interaction task with the environment by using
motion control could be obtained only if the task were accurately planned.
This would in turn require an accurate model of both the robot manipulator
(kinematics and dynamics) and the environment (geometry and mechanical
features). Manipulator modeling can be known with enough precision, but a
detailed description of the environment is difficult to obtain.
To understand the importance of task planning accuracy, it is sufficient to
observe that to perform a mechanical part mating with a positional approach,
the relative positioning of the parts should be guaranteed with an accuracy of an
order of magnitude greater than part mechanical tolerance. Once the absolute
1
2 ROBOT FORCE CONTROL

position of one part is exactly known, the manipulator should guide the motion
of the other with the same accuracy.
In practice, the planning errors may give rise to a contact force causing a
deviation of the end effector from the desired trajectory. On the other hand,
the control system reacts to reduce such deviation. This ultimately leads to a
build-up of the contact force until saturation of the joint actuators is reached or
breakage of the parts in contact occurs.
The higher the environment stiffness and position control accuracy are, the
easier a situation like the one just described can occur. This drawback can
be overcome if a compliant behavior is ensured during the interaction. This
can be achieved either in a passive fashion by interposing a suitable compliant
mechanical device between the manipulator end effector and the environment,
or in an active fashion by devising a suitable interaction control strategy.
The contact force is the quantity describing the state of interaction in the
most complete fashion. Therefore, it is expected that enhanced performance
can be achieved with an interaction control provided that force measurements
are available. To this purpose, a force/torque sensor can be mounted on a robot
manipulator, typically between the wrist and the end effector, and its readings
shall be passed to the robot control unit via a suitable interface.
Robot force control has attracted a wide number of researchers in the past
two decades. A state-of-the-art of the first decade is provided in [111], whereas
the progress of the last decade is surveyed in [105] and [35]. Just recently,
a monograph on force control [48] has appeared. The following section is
aimed at presenting the features of the main strategies of interaction control
with proper reference to the key literature in the field.

2. INDIRECT VS. DIRECT FORCE CONTROL

Interaction control strategies can be grouped in two categories; those per-
forming indirect force control and those performing direct force control. The
main difference between the two categories is that the former achieve force
control via motion control, without explicit closure of a force feedback loop;
the latter, instead, offer the possibility of controlling the contact force to a
desired value, thanks to the closure of a force feedback loop.
To the first category belong compliance (or stiffness) control [82, 87] and
impedance control [50], where the position error is related to the contact force
through a mechanical stiffness or impedance of adjustable parameters. A
robot manipulator under impedance control is described by an equivalent mass-
spring-damper system with the contact force as input. The resulting impedance
in the various task space directions is typically nonlinear and coupled. If a
force/torque sensor is available, then force measurements can be used in the
control law so as to achieve a linear and decoupled impedance.
 Introduction 3

If a detailed model of the environment is available, a widely adopted strat-

egy belonging to the second category is the hybrid position/force control which
aims at controlling position along the unconstrained task directions and force
along the constrained task directions. A selection matrix acting on both de-
sired and feedback quantities serves this purpose for typically planar contact
surfaces [84], whereas the explicit constraint equations have to be taken into
account for general curved contact surfaces [115, 67, 68].
In most practical situations, a detailed model of the environment is not
available. In such a case, an effective strategy still in the second category is
the inner/outer motion/force control where an outer force control loop is closed
around the inner motion control loop which is typically available in a robot
manipulator [36]. In order to embed the possibility of controlling motion along
the unconstrained task directions, the desired end-effector motion can be input
to the inner loop of an inner/outer motion/force control scheme. The resulting
parallel control is composed of a force control action and a motion control
action, where the former is designed so as to dominate the latter in order to
ensure force control along the constrained task directions [24].
The focus of the present book is on force control strategies which are con-
ceived to manage the interaction with a more or less compliant environment
without requiring an accurate model thereof. A simplified environment model
is used to analyze the performance of the various schemes presented. On the
other hand, the environment is an inherent part of the control scheme with the
hybrid strategy which indeed represents the most effective interaction control
for large values of contact stiffness, or an infinitely stiff (rigid) surface in the
limit. For such a reason, hybrid position/force control schemes are not treated
in the book.
The motion control problem is analyzed first not only to understand its
limitations when the end effector interacts with the environment, but also to
enlighten the possibility of achieving an indirect control of the contact force
with a purely motion control strategy. The resulting schemes are compli-
ance control and impedance control, depending whether a static or a dynamic
model-based compensation is performed. Special emphasis is given to control-
ling six-degree-of-freedom interaction tasks involving the end-effector position
and orientation when a contact force and moment is applied. The closure of a
force feedback loop allows direct control of the contact force (moment) along
the constrained task directions, and the parallel composition of the force and
motion control actions also allows control of the end-effector position (orien-
tation) along the unconstrained task directions. Practical issues like imperfect
model-based compensation or lack of speed measurements are challenged by
devising advanced force and position control schemes which respectively resort
to parameter adaptation and output feedback.
4 ROBOT FORCE CONTROL

Figure 1.1. Experimental apparatus available in the PRISMA Lab.

In order to validate the theoretical findings, most of the force control schemes
presented throughout the book are tested in a number of experiments for rep-
resentative interaction tasks. Indeed, the force/torque sensor needed for the
execution of interaction control schemes is not typically available for industrial
robots, because of high cost and scarce reliability. A commercially available
industrial robot has been purposefully utilized to demonstrate the credibility
of force control in the perspective of the next generation of robot control units
with enhanced sensory feedback capabilities. The problem of interfacing the
force/torque sensor has been solved thanks to the open architecture of the
control unit. This feature is crucial also for the implementation of control
schemes requiring model-based compensation actions. The apparatus used for
experimental testing is described in the next section.

3. EXPERIMENTAL APPARATUS
The experimental apparatus available in the PRISMA Lab comprises two
industrial robots Comau SMART-3 S (Fig. 1.1).
Each robot manipulator has a six-revolute-joint anthropomorphic geometry
with nonnull shoulder and elbow offsets and non-spherical wrist. One manip-
ulator is mounted on a sliding track which provides an additional degree of
mobility. The joints are actuated by brushless motors via gear trains; shaft
absolute resolvers provide motor position measurements. Each robot is con-
 Introduction 5

Figure 1.2. Force/torque sensor mounted at robot wrist.

trolled by the C3G 9000 control unit which has a VME-based architecture
with 2 processing boards (Robot CPU and Servo CPU) both based on a Mo-
torola 68020/68882, where the latter has an additional DSP and is in charge of
trajectory generation, inverse kinematics and joint position servo control. Inde-
pendent joint control is adopted where the individual servos are implemented
as standard PID controllers. The native robot programming language is PDL 2,
a high-level Pascal-like language with typical motion planning instructions.
An open control architecture is available which allows testing of advanced
model-based control algorithms on a conventional industrial robot [37]. Con-
nection of the VME bus of the C3G 9000 unit to the ISA bus of a standard PC
is made possible by a BIT 3 Computer bus adapter board, and the PC and C3G
controller communicate via the shared memory available in the Robot CPU;
the experiments reported in the present book have been carried out by using
a PC Pentium MMX/233. Time synchronization is implemented by interrupt
signals from the C3G to the PC with data exchange at a given sampling rate. A
set of C routines are available to drive the bus adapter boards. These routines
are collected in a library (PCC3Link) and are devoted to performing communi-
cation tasks, e.g., reading shaft motor positions and/or writing motor reference
currents from/to the shared memory. Also, a set of routines are devoted to per-
forming safety checks and monitoring system health, e.g., a watchdog function
and/or maximum current checks.
Seven different operating modes are available in the control unit, allowing
the PC to interact with the original controller both at trajectory generation level
and at joint control level. To implement model-based control schemes, the
operating mode number 4 is used in which the PC is in charge of computing the
control algorithm and passing the references to the current servos through the
communication link at 1 ms sampling time. Joint velocities are reconstructed
through numerical differentiation of joint position readings, except when a
specific observer is used. All control algorithms are implemented in discrete
time at 1 ms sampling interval.
6 ROBOT FORCE CONTROL

FORCE
PENTIUM PARALLEL READINGS

233 INTERFACE

ISA BUS

BIT 3
ADAPTER
PC

C3G 9000 BIT 3

ADAPTER

VME BUS

JOINT
READINGS
ROBOT SERVO
CPU CPU
MOTOR
CURRENTS

Figure 1.3. Schematic of open control architecture.

A six-axis force/torque sensor ATI FT30-100 with force range of 130 N and
torque range of 10 N
m can be mounted at the wrist of either robot (Fig. 1.2).
The sensor is connected to the PC by a parallel interface board which provides
readings of six components of generalized force at 1 ms.
A schematic of the open control architecture is depicted in Fig. 1.3.
The type of environment considered in the various case studies presented in
the following chapters consists of planar compliant surfaces of medium stiff-
ness, with an estimated contact stiffness of the order of 104 N/m for translational
displacements and of 10 Nm/rad for rotational displacements. This choice is
motivated by the desire of safely analyzing the performance of the various
control schemes where the interaction with the environment encompasses an
unplanned transition from non-contact to contact at nonnegligible end-effector
speed.
Chapter 2

MOTION CONTROL

Prior to tackling the force control problem, the motion control problem is analyzed.
Kinematic and dynamic modeling of a robot manipulator are presented. The inverse
dynamics strategy is pursued leading to a resolved acceleration control which ensures
tracking of a desired end-effector position and orientation trajectory. Different types
of orientation error are considered which are based on Euler angles, angle/axis and
quaternion, respectively. A redundancy resolution scheme is embedded into the control.
Regulation to a desired end-effector position and orientation is achieved by a PD control
with gravity compensation.

1. MODELING
The class of robotic systems considered throughout this book is that of robot
manipulators. The mechanical structure of a robot manipulator consists of a
sequence of links connected by means of joints. Links and joints are usually
made as rigid as possible to achieve high precision in robot positioning. An end
effector is mounted at the tip of the manipulator which is specified according
to the task the robot shall execute.
Completion of a generic robot task requires the execution of a specific
motion prescribed to the manipulator end effector. The motion can be either
unconstrained, if there is no physical interaction between the end effector and
the environment, or constrained if contact forces arise between the end effector
and the environment.
The correct execution of the end-effector motion is entrusted to the con-
trol system which shall provide the joint actuators of the manipulator with the
commands consistent with the mechanical structure of the manipulator. Mod-
eling a robot manipulator is therefore a necessary premise to finding motion
control strategies. In the following, the kinematic model and the dynamic
7
8 ROBOT FORCE CONTROL

n e

q4 s
e

q3 a e

q2
p
e

Z
q1 b

O b

X b
Y
b

Figure 2.1. Schematic of an open-chain robot manipulator with base frame and end-effector
frame.

model of a robot manipulator are concisely presented. The reader is referred

to Appendix A for background material on rigid body orientation.

1.1 KINEMATICS
A robot manipulator consists of a kinematic chain of n +1 links connected by
means of n joints. Joints can essentially be of two types: revolute and prismatic,
while complex joints can be decomposed into these simple joints. Revolute
joints are usually preferred to prismatic joints in view of their compactness and
reliability. One end of the chain is connected to the base link, whereas an end
effector is connected to the other end. The basic structure of a manipulator is
the open kinematic chain which occurs when there is only one sequence of links
connecting the two ends of the chain. Alternatively, a manipulator contains a
closed kinematic chain when a sequence of links forms a loop. In Fig. 2.1, an
open-chain robot manipulator is illustrated with conventional representation of
revolute and prismatic joints.
q p
Let denote the (n  1) vector of joint variables, e the (3  1) position
vector, and
R
e = [ e e e] n s a (2.1)

the (3  3) rotation matrix, describing the origin and the orientation of the
n s a
end-effector frame, where e , e , e are the unit vectors of the axes of the end-
p R
effector frame (Fig. 2.1). The quantities e and e characterize the end-effector
frame e (Oe –Xe Ye Ze ) with respect to a fixed base frame b (Ob –Xb Yb Zb ) and
no superscript is used; instead, if a matrix or vector quantity is to be referred to
 Motion Control 9

a frame other than the base frame, then a proper frame superscript shall precede
the quantity.
The kinematic model of the manipulator gives the relationship between q
p
and e , i.e.
p p q
e = e( ) (2.2)
as well as between q and Re, i.e.
Re = Re(q). (2.3)

q p
Let _ denote the vector of joint velocities, _ e the (3  1) vector of end-effector
!
linear velocity, and e the (3  1) vector of end-effector angular velocity. The
differential kinematics model gives the relationship between _ andq
ve = !p_ ee
 
(2.4)

in the form
ve = J (q)q_ (2.5)
J
where is the (6  n) end-effector geometric Jacobian matrix. The Jacobian
can be partitioned as
J = JJ po
 
(2.6)
to separate the contributions of the joint velocities to the linear and the angular
J
velocity in (2.4). The joint configurations at which the matrix is not full-rank
are termed kinematic singularities.
The angular velocity is related to the time derivative of the rotation matrix
by the notable relationship
R_ e = S (!e)Re (2.7)
S
where is defined as in (A.4).
The description of end-effector orientation by a rotation matrix is nonmin-
imal since the nine elements of the matrix are subject to the six constraints
expressed by (A.1), and thus an arbitrary orientation can be completely speci-
fied in terms of three degrees of freedom. Since an arbitrary position is specified
in terms of three independent coordinates, it follows that a general motion task
for the end-effector position and orientation requires m degrees of freedom
with m  6. Whenever the number of joints exceeds the number of degrees of
freedom, i.e. n > m, the manipulator is said kinematically redundant. Notice
that redundancy is a concept relative to the end-effector task; for instance, a
six-joint manipulator becomes redundant relative to a laser-cutting task since
a
the end-effector rotation about the approach direction ( e ) is irrelevant to com-
pletion of the task. On the other hand, a seven-joint manipulator is inherently
redundant with respect to any end-effector task.
10 ROBOT FORCE CONTROL

1.2 DYNAMICS
Dynamic modeling of a robot manipulator consists of finding the relationship
between the forces exerted on the structure and the joint positions, velocities
and accelerations. The dynamic model can be derived using the Lagrange for-
mulation. Since the joint variables qi constitute a set of generalized coordinates
for the mechanical system, the Lagrange equations can be written as
d @L ? @L =  i = 1; : : : ; n
dt @ q_i @qi i (2.8)

where
L = T ?U (2.9)
is the Lagrangian given by the difference between kinetic energy and potential
energy, and i is the generalized force at joint i; a torque for a revolute joint and
a force for a prismatic joint, respectively. Typically the generalized forces are
shortly referred to as torques, since most joints of a manipulator are revolute.
The kinetic energy is a quadratic form of the joint velocities, i.e.

T = 21 q_ T B(q)q_ (2.10)

Bq
where the (n  n) matrix ( ) is the inertia matrix of the robot manipulator
which is symmetric and positive definite. Substituting (2.10) in (2.9) and taking
the derivatives needed by (2.8) leads to the equations of motion
B(q)q + C (q; q_ )q_ + g(q) =  (2.11)
where  is the (n  1) vector of joint torques, g (q ) is the (n  1) vector of
gravity torques with
@U ,
gi (q) = @q (2.12)
i
and  T
_ 1 @
C (q; q_ )q_ = B(q; q_ )q_ ? 2 @ q (q_ B(q)q_ )
T
(2.13)

is the (n  1) vector of Coriolis and centrifugal torques. This term is quadratic

in the joint velocities, and thus its generic element can be written as
n
X
Cij = cijk q_j q_k . (2.14)
k=1
C
There exist several choices of the elements Cij of the matrix satisfying (2.14)
Cqqq
corresponding to different factorizations of the term ( ; _ ) _ . The choice
!
cijk = 12 @B ij + @Bik ? @Bjk
@qk @qj @qi , (2.15)
 Motion Control 11

where the cijk ’s are termed Christoffel symbols of the first type, makes the
B C
matrix _ ? 2 skew-symmetric; this property is very useful for control design
purposes.
Regarding the joint torque, each joint is driven by an actuator (direct drive
or gear drive); in general, the following torque contributions appear

i = i ? fi ? ei (2.16)

where i is the driving torque at the joint, fi is the torque due to joint friction,
and ei is the torque caused by the external force and moment exerted by the
end effector when in contact with the environment. Note that the actuators
(electric or hydraulic) have been assumed as ideal torque generators.
Joint friction is difficult to model accurately; as a simplified model, only
viscous friction is considered, i.e.

 f = F q_ (2.17)

F
where is a positive definite (diagonal) matrix of viscous friction coefficients
at the joints. More complex models would include nonlinear phenomena such
as Coulomb friction and Stribeck effects.
f
Finally, let denote the (3  1) vector of external end-effector force and 
the (3  1) vector of external end-effector moment. By applying the principle
of virtual work, the resulting joint torques are

 e = J T(q)h (2.18)

where
h = f
 
(2.19)

J
and is defined in (2.5).
In view of (2.16), (2.17) and (2.18), the dynamic model can be written in the
form
B(q)q + C (q; q_ )q_ + F q_ + g(q) =  ? J T(q)h. (2.20)

It can be shown that equation (2.20) can be cast in a linear form with respect to

a suitable (p  1) vector of dynamic parameters as

Y (q; q_ ; q) =  ? J T(q)h (2.21)

where the (n  p) matrix Y is termed regressor of the dynamic model. In

general, the dynamic parameters depend on the mass, first moment of inertia
and inertia tensor of each link, and the friction coefficient of each joint.
12 ROBOT FORCE CONTROL

2. TRACKING CONTROL
The motion control problem for a robot manipulator can be formulated as
finding the joint torques which ensure that the end effector attains a desired
position and orientation. The usual way of solving this problem when the end
effector is not in contact with the environment consists of two stages; first the
manipulator kinematics is inverted to compute the joint variables corresponding
to the given end-effector position and orientation, then a controller is designed
which guarantees that the joints attain the computed values. This strategy is
known as kinematic control.
Since the present book is focused on the problem of controlling the inter-
action between the manipulator end effector and the environment, a different
strategy is pursued which consists of a single stage, i.e. the design of a so-called
task space control. Direct task space feedback is utilized where the end-effector
position and orientation is computed via the kinematics relationships from the
joint measurements.
More specifically, the goal can be either to follow a time-varying desired
position and orientation, i.e. a tracking control problem, or to reach a constant
desired position and orientation, i.e. a regulation problem. The former problem
is treated next, while the latter problem will be treated in Section 3.

2.1 DYNAMIC MODEL-BASED COMPENSATION

The dynamic model in the form (2.20) is represented by a set of n second-
order coupled and nonlinear differential equations relating the joint positions,
velocities and accelerations to the joint torques and the end-effector force and
moment. A classical strategy to control this type of mechanical system is inverse
dynamics control, which is aimed at linearizing and decoupling the manipulator
dynamics via feedback. Nonlinearities such as Coriolis and centrifugal torques,
friction torques, and gravity torques can be merely cancelled by adding these
terms to the control input, while decoupling can be achieved by weighting the
control input by the inertia matrix. According to this dynamic model-based
compensation, the joint torques can be chosen as
 = B(q) + C (q; q_ )q_ + F q_ + g(q) (2.22)
where it has been assumed h = 0, i.e. no interaction with the environment,
while  constitutes a new control input to be properly designed.
Folding the control law (2.22) into the system model (2.20), and taking into
account that B (q ) is always nonsingular, yields
q =  (2.23)
which constitutes a linear and decoupled system corresponding to a double
 q 
integrator between the input and the output . The quantity represents a
resolved acceleration in terms of joint variables.
 Motion Control 13

Equation (2.23) has been obtained under the assumption of perfect com-
pensation of the terms in (2.20). This relies on the availability of an accurate
dynamic model, as can be obtained via an identification procedure of the dy-
namic parameters in (2.21). In case of imperfect compensation, a mismatching
occurs which causes the presence of a disturbance term in (2.23), i.e.
q =  ? ; (2.24)

in practice, the disturbance is mainly due unmodeled dynamics, such as
imperfect compensation of friction torques since they are difficult to model
accurately.
As pointed out above, the goal is to design a position control action ensuring
tracking of a desired end-effector position trajectory as well as an orientation
control action ensuring tracking of a desired end-effector orientation trajectory.
q
Since Equation (2.23) contains  , it is worth considering the time derivative
of (2.5), i.e.
v Jqq Jqqq
_ e = ( ) + _ ( ; _ ) _ (2.25)
which provides the relationship between the joint accelerations and the end-
effector linear and angular accelerations.

At this point, the new control input in (2.23) can be chosen as
 
 = J ?1(q) a ? J_ (q; q_ )q_ (2.26)
which in view of (2.25) leads to
v_ e = a (2.27)
a
where attains the meaning of a resolved acceleration in terms of end-effector
variables.
In deriving (2.26), a nonredundant manipulator (n = 6) moving in a
singularity-free region of the workspace has been considered to compute the
inverse of the Jacobian. A damped least-squares inverse can be adopted to gain
robustness in the neighborhood of kinematic singularities, whereas a pseudo-
inverse can be used in the redundant case (n > 6) in conjunction with a
suitable term in the null space of the Jacobian describing the internal motion
of the manipulator, as shall be seen in Subsection 2.6.
In the case of the presence of a disturbance as in (2.24), Equation (2.27)
shall be modified into
v
_e = ? a d (2.28)
d Jq
where = ( ) .
v
In view of the partition of e in (2.4), it is appropriate to partition the vector a
into its linear and angular components, i.e.
a
a = ao

p

(2.29)
14 ROBOT FORCE CONTROL

a a
where p and o are (3  1) vectors. Therefore, Equation (2.27) can be rewritten
as
p e = ap (2.30)
!_ e = ao , (2.31)
a a
where p and o shall be designed so as to ensure tracking of the desired
end-effector position and orientation trajectory, respectively.
The desired position trajectory is specified in terms of the position vec-
p p p
tor d (t), linear velocity vector _ d (t) and linear acceleration vector  d (t),
whereas the desired orientation trajectory is specified in terms of the rotation
R !
matrix d (t), angular velocity vector d (t) and angular acceleration vec-
! p R
tor _ d (t). The quantities d and d characterize the origin and the orientation
of a desired frame d .
A position error between the desired and the actual end-effector position
can be defined as
p

de = d ? e p p (2.32)
where the operator
denotes that a vector difference has been taken, and the
double subscript denotes the corresponding frames. Then, the resolved linear
acceleration can be chosen as
ap = p d + K Dp
p_ de + K Pp
pde (2.33)
K K
where Dp and Pp are suitable feedback matrix gains. Substituting (2.33)
into (2.30) gives the closed-loop dynamic behavior of the position error

pde + K Dp
p_ de + K Pp
pde = 0. (2.34)
The system (2.34) is exponentially stable for any choice of positive definite
K K p
Dp and Pp , and thus tracking of d and _ d is ensured. p
a
As regards the resolved angular acceleration, o can be chosen in different
ways, depending on the definition of end-effector orientation error used. There-
fore, in the following three subsections, various definitions of orientation error
are considered which make use of Euler angles, angle/axis and quaternion,
respectively. The derivation of the control laws relies on the mathematical
background that can be found in Appendix A.

2.2 EULER ANGLES ERROR

The most natural way of defining an orientation error is to consider an
expression analogous to the position error in (2.32), i.e.

'de = 'd ? 'e (2.35)
' '
where d and e are the set of Euler angles that can be extracted respectively
R R
from the rotation matrices d and e describing the orientation of d and e .
 Motion Control 15

!
Since Equation (2.31) contains _ e , it is worth considering the relationship
'
between the time derivative of the Euler angles _ e and the end-effector angular
!
velocity e , i.e.
! T' '
e = ( e) _ e (2.36)
T
where is a transformation matrix that depends on the particular set of Euler
angles considered; this matrix suffers from two representation singularities.
The time derivative of (2.36) yields the acceleration relationship in the form
!_ e = T ('e)'e + T_ ('e; '_ e)'_ e. (2.37)

As concerns the desired end-effector orientation trajectory in terms of Euler

'
angles, the time derivative of d can be computed as
'_ d = T ?1('d )!d, (2.38)
whereas the time derivative of '_ d can be computed as
 
' d = T ?1 ('d) !_ d ? T_ ('d ; '_ d)'_ d , (2.39)

T'
with obvious reference to (2.36) and (2.37); the matrix ( d ) in (2.38)
and (2.39) has been assumed to be nonsingular.
In view of (2.37), the resolved angular acceleration based on the Euler angles
error can be chosen as
ao = T ('e)('d + K Do
'_ de + K Po
'de) + T_ ('e; '_ e)'_ e (2.40)
where K Do and K Po are suitable feedback matrix gains. Substituting (2.40)
into (2.31) gives the closed-loop dynamic behavior of the orientation error

'de + K Do
'_ de + K Po
'de = 0, (2.41)
where (2.37) has been used assuming that the matrix T ('e ) is nonsingular. The
system (2.41) is exponentially stable for any choice of positive definite Do K
K ' '
and Po ; tracking of d and _ d is ensured, which in turn implies tracking
R !
of d and d .
The above Euler angles error becomes ill-conditioned when the actual end-
'
effector orientation e becomes close to a representation singularity. In order
to overcome this drawback, an alternative Euler angles error can be consid-
ered which is based on the rotation matrix describing the mutual orientation
between d and e , i.e.
R
e = T
d RR
e d (2.42)
as in (A.8). Differentiating (2.42) with respect to time and accounting for (A.9)
gives
R d S ! R
e _ = (
e )e
de d (2.43)
16 ROBOT FORCE CONTROL

where

e !de = e !d ? e !e (2.44)
is the end-effector angular velocity error which has been referred to e .
' R
Let de denote the set of Euler angles that can be extracted from e d . Then,
!
in view of (2.36), the angular velocity
e de in (2.43) is related to the time
'
derivative of de as
! T' '

e de = ( de ) _ de . (2.45)
The time derivative of (2.45) gives the acceleration relationship in the form
!_ e = !_ d ? T e('de)'de ? T_ e('de ; '_ de)'_ de (2.46)
where
T e('de) = ReT ('de). (2.47)
In view of (2.46), the resolved angular acceleration can be chosen as
ao = !_ d + T e('de)(K Do'_ de + K Po'de) ? T_ e('de ; '_ de)'_ de (2.48)
where K Do and K Po are suitable feedback matrix gains. Substituting (2.48)
into (2.31) gives the closed-loop dynamic behavior of the orientation error
' de + K Do'_ de + K Po'de = 0 (2.49)
where (2.46) has been used assuming that the matrix T e ('de ) is nonsingular.
The system (2.49) is exponentially stable for any choice of positive definite
K Do and K Po; convergence to 'de = 0 and '_ de = 0 is ensured, which in
turn implies tracking of Rd and ! d .
The clear advantage of the alternative over the classical Euler angles error
based on (2.35) is that, by adopting a representation 'de = [ de de de ]T
for which T (0) is nonsingular, representation singularities occur only for large
orientation errors, e.g. when de = =2 for the XYZ representation in (A.11).
Notice that it is not advisable to choose the widely-adopted ZYZ representation
'
which is singular right at de = 0 (see the transformation matrix in (A.18))! In
T
other words, the ill-conditioning of matrix is not influenced by the desired or
actual end-effector orientation but only by the orientation error; hence, as long
as the error parameter jde j < =2, the behavior of system (2.31) and (2.46) is
not affected by representation singularities.

2.3 ANGLE/AXIS ERROR

A different definition of orientation error can be obtained using an angle/axis
representation. In view of (2.42), the mutual orientation between d and e is
R
described by e d , and thus the orientation error can be defined in terms of the
general expression
o
e = f (# )e ,
de de de r (2.50)
 Motion Control 17

r
where #de and e de are respectively the rotation and the unit vector correspond-
R
ing to e d , and f (
) is a suitable scalar function with f (0) = 0. Common
choices for f (#) are summarized in Tab. 2.1.

Table 2.1. Common choices for f (#).

Representation f (#)
Classical angle/axis sin(#)
Quaternion sin(#=2)
Rodrigues parameters tan(#=2)
Simple rotation #

In the following, the classical angle/axis representation is adopted, while the

quaternion will be treated in the next subsection. Hence, the angle/axis error
is
o
e 0 = sin(# )e
de de de r (2.51)
which is usually referred to the base frame, i.e.

o0de = Reeo0de. (2.52)

A computational expression of o0de in terms of the rotation matrices of d

and e can be derived in the form

o0de = 12 (S (ne)nd + S(se)sd + S (ae)ad) . (2.53)

The time derivative of the orientation error (2.53) can be related to the
angular velocities of d and e as

o_ 0de = LT!d ? L!e (2.54)

where

L = ? 12 (S (nd)S (ne) + S (sd )S (se) + S (ad)S (ae)) . (2.55)

The second time derivative is

o 0de = LT!_ d + L_ T!d ? L!_ e ? L!

_ e. (2.56)

The resolved angular acceleration can be chosen as

 
ao = L?1 LT!_ d + L_ T!d ? L!
_ e + K Do o_ 0de + K Po o0de , (2.57)
18 ROBOT FORCE CONTROL

K K L
where Do and Po are suitable feedback matrix gains, and is nonsingular
provided that the angle #de belongs to the interval (?=2; =2); it can be
nn ss
e d > 0, e d > 0,
shown that this restriction is equivalent to the conditions T T

aa
e d > 0.
T

Substituting (2.57) into (2.31) gives the closed-loop dynamic behavior of the
orientation error
o K o K o
 0de + Do _ 0de + Po 0de = 0, (2.58)
which is a linear and decoupled system analogous to the position error sys-
tem (2.34) as well to the orientation error systems (2.41) and (2.49). Exponen-
K
tial stability is guaranteed for any choice of positive definite Do and Po ; K
o o
convergence to 0de = 0 and _ 0de = 0 is ensured, which in turn implies tracking
R
of d and d . !
Equation (2.57) reveals that the price to pay to obtain a linear and decoupled
system is a large computational burden and the possible occurrence of a singu-
larity. On the other hand, a simpler angle/axis scheme based on the error (2.53)
can be devised where the resolved angular acceleration is chosen as

ao = !_ d + K Do
!de + K Poo0de. (2.59)

In this case, the closed-loop dynamic behavior of the orientation error becomes

!_ de + K Do
! de + K Po o0de = 0. (2.60)

Differently from all the previous cases (2.34), (2.41) and (2.49), the error
system is nonlinear, and thus a Lyapunov argument is invoked below to ascertain
its stability. To this purpose, in view of (A.29) and (A.30), the orientation
error (2.51) can be expressed in terms of a quaternion as
e o0 = 2 e 
de de de (2.61)


where fde ; e de g can be extracted from (2.42). Furthermore, the feedback
K I K
gains are taken as scalar matrices, i.e. Do = kDo and Po = kPo where I
I denotes the (3  3) identity matrix. Let

V = 2kPo eTdeede + 12
!Tde
!de (2.62)

be a positive definite Lyapunov function candidate. The time derivative

of (2.62) along the trajectories of system (2.60) is

V_ = 4kPoeTdee_ de +
!Tde
!_ de (2.63)

= 2kPo e Tde E (de ; e de )
e ! de ? kDo
!Tde
!de ? 2kPo de
e !Tde e de
= ?kDo
!Tde
! de
 Motion Control 19

where the propagation rule based on (A.46) and (A.47) has been exploited, i.e.

_de = ? 12 e Tde
e ! de (2.64)

e _ 1 E ( ; e  )
e !
de = 2 de de de (2.65)

E
with defined as in (A.38).
Since V_ is only negative semi-definite, in view of LaSalle theorem, the
system asymptotically converges to the invariant set described by the following
equilibria:
E1 = fde = 1; ede = 0;
!de = 0g (2.66)
E2 = fde = ?1; de = 0;
!de = 0g
e (2.67)
E3 = fde = 0; de : k dek = 1;
!de = 0g:
e e (2.68)
The equilibria in the set E3 are all unstable. To see this, consider (2.62) which,
in view of (2.63), is a decreasing function. At any equilibrium in (2.68), it is
V1 = 2kPo: (2.69)
Take a small perturbation de =  around such equilibrium; then, it is
e T e de = 1 ? 2 . The perturbed Lyapunov function is
de
V = 2kPo ? 22 kPo < V1 (2.70)
and thus, since (2.62) is decreasing, V will never return to V1 , implying that the
equilibria in E3 are unstable. Therefore, the system asymptotically converges
to either E1 or E2 ; since both quaternions for those equilibria represent the same
R !
orientation, it can be concluded that tracking of d and d is achieved.
It is worth emphasizing that, compared to the previous angle/axis scheme
based on (2.57), the restriction on #de does no longer hold. Nevertheless, for
large values of #de , the angle/axis scheme based on (2.59) may lead to alignment
of d and e with #de = 2 as in the equilibrium E2 ; such occurrence is
of no practical interest since #de is typically small for the tracking control at
issue.

2.4 QUATERNION ERROR

A special type of angle/axis representation of the orientation error is obtained
with the quaternion, i.e.

o00de = sin #2de erde = ede (2.71)

corresponding to the vector part of the quaternion that can be extracted from
R
the rotation matrix e d in (2.42) (see (A.39)).
20 ROBOT FORCE CONTROL

The relationship with the angular velocity error in (2.44) is established

by (2.65).
The resolved angular acceleration based on the quaternion error can be
chosen as
a ! K ! K R 
o = _ d + Do
de + Po e e de (2.72)
where K Do and K Po are suitable feedback matrix gains, and the orientation
error has been referred to the base frame. Substituting (2.72) into (2.31) gives
the closed-loop dynamic behavior of the orientation error

!_ de + K Do
! de + K PoRe e de = 0. (2.73)

Similarly to the angle/axis case above, a Lyapunov argument is invoked

below to ascertain stability of system (2.73). Again, the feedback gains are
K I K I
taken as scalar matrices, i.e. Do = kDo and Po = kPo . Let

V = kPo (de ? 1)2 + eTdeede + 21
!Tde
!de

 
(2.74)

be a positive definite Lyapunov function candidate. The time derivative

of (2.74) along the trajectories of system (2.73) is
 
V_ = 2kPo (de ? 1)_de + eTdee_ de +
!Tde
!_ de (2.75)
 
= kPo ?(de ? 1)e Tde
e !de + e Tde E (de ; e de )
e ! de
?kDo
!Tde
!de ? kPo
e!Tdeede
= ?kDo
!Tde
!de
where (2.64) and (2.65) have been exploited.
Since V_ is only negative semi-definite, in view of LaSalle theorem, the system
asymptotically converges to the invariant set described by the two equilibria E1
and E2 in (2.66) and (2.67).
The equilibrium E2 is unstable. To see this, consider (2.74) which, in view
of (2.75), is a decreasing function. At the equilibrium in (2.67), it is
V1 = 4kPo . (2.76)
Take a small perturbation de = ?1 +  around the equilibrium with  > 0;
then, it is e T
de de = 2 ?  . The perturbed Lyapunov function is
e 2

V = 4kPo ? 2kPo < V1 (2.77)

and thus, since (2.74) is decreasing, V will never return to V1 , implying that E2
is unstable. Therefore, the system must asymptotically converge to E1 , which
in turn implies that tracking of Rd and !d is achieved.
 Motion Control 21

2.5 COMPUTATIONAL ISSUES

The features of the various control schemes can be further investigated from
a computational viewpoint. Since the main differences reside in the way the
orientation error is defined, the analysis can be focused on the issues regarding
the computation of the resolved angular acceleration. In this respect, it is
assumed that the robot manipulator is controlled in a real-time fashion, and
thus the planning of the desired end-effector trajectory shall be updated on
the basis of sensory information about the surrounding environment where
the robot operates. Therefore, the two key elements in the analysis are the
trajectory generation and the computation of the orientation error.
The most congenial way for the user to specify a desired orientation trajectory
is the angle/axis method. Given the rotation matrices describing the initial and
final orientation of d , the rotation matrix of the mutual orientation is computed;
the angle and axis unit vector are extracted, and the orientation trajectory is
interpolated from zero to the total angle while keeping the unit vector constant.
This method provides the user with a meaningful interpretation of the end-
effector orientation along the trajectory from a geometric viewpoint, and thus
it is superior to the computationally lighter Euler angles method for which
the intermediate orientations of d cannot be predicted in advance. On the
other hand, assigning the angle and axis directly leads to specifying a desired
trajectory in terms of a quaternion via (A.29) and (A.30). Upon these premises,
it is assumed that the desired end-effector orientation trajectory is generated
R
in terms of the rotation matrix d via (A.21); then, the angular velocity d !
!
and acceleration _ d are simply computed from the time derivatives of the
interpolating polynomial for the angle.
Regarding the actual orientation of e , this is typically available from the
R
direct kinematics equation in terms of the rotation matrix e which can be
computed from the joint position measurements via (2.3); further, the actual
!
end-effector angular velocity e can be computed from the joint velocity mea-
surements via (2.5).
The computational burden of the schemes presented in the previous subsec-
tions has been evaluated in terms of the number of floating-point operations
and transcendental functions needed to compute the resolved angular acceler-
a
ation o . The angle/axis scheme based on (2.57) has been ruled out in view of
its inherent computational complexity, and thus the scheme based on (2.59) has
been considered hereafter. The results are reported in Tab. 2.2, where the addi-
tional computations needed for desired trajectory generation by those schemes
R ! !
not using d , d and _ d are evidenced; the computations have been optimized
whenever possible, e.g. by avoiding multiplications by zero and carrying out
partial factorizations.
It can be recognized that the extraction of Euler angles augments the overall
load for both schemes, compared to the quaternion and the angle/axis scheme.
22 ROBOT FORCE CONTROL

Table 2.2. Computational load of the angular part for the resolved acceleration control schemes.

Resolved acceleration Trajectory generation

Orientation error Flops Funcs Flops Funcs
Classical Euler angles 68 8 52 8
Alternative Euler angles 136 8 0 0
Angle/axis 55 0 0 0
Quaternion 60 1 21 1

The interesting feature of the alternative Euler angles scheme is the absence of
extra computations for trajectory generation, which instead penalizes the clas-
R
sical scheme due to the extraction of Euler angles from d and the computation
of their time derivatives. The computational load for the quaternion scheme
is smaller than for the Euler angles schemes, even though an additional effort
is required to compute the desired trajectory in terms of a quaternion. Not
surprisingly, the angle/axis scheme is the most computationally efficient since
it operates directly on the desired and actual rotation matrices, the desired and
actual angular velocities, and the desired angular acceleration.
In conclusion, a critical discussion is in order concerning the pros and cons
of each scheme.
At first sight, the classical Euler angles scheme might seem the simplest one
in view of its similarity with the position scheme. Nevertheless, the analysis
has revealed that, besides the heavy computational load due to Euler angles
extraction, there is no guarantee to avoid the occurrence of representation
singularities even when good end-effector orientation tracking is achieved. On
the other hand, the effort to plan a singularity-free orientation trajectory is
considerable especially when the angle/axis method is adopted for meaningful
task specification purposes.
The alternative Euler angles scheme has the main merit to almost overcome
the above drawback of representation singularities, since it keenly operates
on the set of Euler angles which is extracted from a single rotation matrix
describing the mutual orientation between d and e . It may suffer only
in the case of large orientation errors, but there is no practical worry for
a convergent algorithm with matched initial conditions between the desired
and the actual end-effector orientation. A weakness, however, is that the
computational burden is still considerable.
The breakthrough of the quaternion scheme stands in its applicability for any
magnitude of the orientation error, since it is inherently based on a singularity-
free representation of orientation. Its tracking performance is apparently as
good as the alternative Euler angles scheme, although the closed-loop ori-
entation error dynamics is nonlinear. A further advantage is represented by
 Motion Control 23

the contained computational burden, even though the orientation error is not
directly based on the desired and actual rotation matrices.
Finally, the angle/axis scheme has the least computational load among all
the schemes. Its performance is worse than that of the quaternion scheme in the
case of large orientation errors, whereas both schemes exhibit the same good
behavior for small orientation errors.

2.6 REDUNDANCY RESOLUTION

In case the manipulator is kinematically redundant, there exist infinite joint
motions that produce the same end-effector motion. In particular, even when the
end effector is at rest, it is possible to generate an internal motion at the joints.
As a minimal requirement, such motion should be made stable. In addition,
it could be keenly utilized to meet additional task requirements besides the
execution of the end-effector trajectory, thus providing redundancy resolution.
Redundancy can be solved either at kinematic level, that is in the first stage
of a kinematic control strategy, or at dynamic level by suitably modifying the
inverse dynamics control law. The latter approach is pursued hereafter since
task space control is of interest in the present book.
Since the Jacobian matrix for a redundant manipulator has more columns
J
than rows (n > 6), a suitable right inverse of is to be used in lieu of ?1 . J
Hence, in lieu of (2.26), the new control input in (2.22) can be chosen as
 
 = J y(q) a ? J_ (q)q_ + n (2.78)

where
J y = W ?1 J T JW ?1J T ?1
 
(2.79)

denotes the right pseudo-inverse of J weighted by the positive definite (n  n)

matrix W . Also, in (2.78), n denotes a joint acceleration vector lying in
the null space of J which is available for redundancy resolution. In fact,
plugging (2.78) in (2.23) gives
 
q = J y(q) a ? J_ (q)q_ + n. (2.80)

Then, premultiplying both sides of (2.80) by J , accounting for (2.25), and

observing that JJ y = I , Jn = 0, yields the same end-effector resolved
acceleration as in (2.27); I denotes the (n  n) identity matrix.
The matrix projecting arbitrary joint accelerations into the null space of
J is given by (I ? J yJ ), no matter what choice is made for the weighting
matrix W in (2.79). Therefore, it is significant to choose W so that the
redundancy resolution scheme for motion control should not be altered when
interaction with the environment occurs. To this purpose, from (2.20) the joint
24 ROBOT FORCE CONTROL

accelerations induced by the external end-effector force and moment are given
by
q B qJ qh
 e = ? ?1 ( ) T ( ) . (2.81)
Projecting these accelerations in the null space of the Jacobian gives
 
qen = ? I ? J y(q)J (q) B?1(q)J T(q)h. (2.82)

Choosing W = B in (2.79) and plugging the resulting J y in (2.82) yields

qen = 0, meaning that the external force and moment produce no null space
joint accelerations. Therefore, in view of this choice, in the design of the joint
resolved acceleration in (2.78) the vector n can be used to solve redundancy
independently of the occurrence of interaction with the environment. The
matrix
?1 T ?1
 
J
y = ?1 T B J JB J (2.83)
weighted by the inertia matrix is termed dynamically consistent pseudo-inverse
of the Jacobian.
The next step consists of designing a redundancy resolution control in terms

of the null space joint accelerations n in (2.78). To this purpose, n shall
be chosen so as to ensure stabilization of the null space motion and possibly
optimization of an additional task function. Let
 
en = I ? J y(q)J (q) ( ? q_ ) (2.84)

denote the null space velocity error where  is a joint velocity vector which
is available for redundancy resolution. The goal is to make en asymptotically
converge to zero. Taking the time derivative of (2.84) and using (2.80) gives
the null space dynamics, i.e.
   
e_ n = I ? J y(q)J (q) (_ ? n) ? J_ y(q)J (q) + J y(q)J_ (q) ( ? q_ )
(2.85)
y
where J_ is a shortcut notation for the time derivative of J y .
Consider the Lyapunov function candidate

V = 12 eTn B(q)en. (2.86)

Computing the time derivative of (2.86) along the trajectories of system (2.85)
yields
V_ = 12 eTn Be
 
_ n + eTn B _ ? n ? J_ y J ( ? q_ ) , (2.87)
where the dependence on q has been dropped off, and the identity
eTn BJ y = 0T (2.88)
 Ob

Xb

Yb

Zb

q1
q2
q3
q4
p Motion Control 25
e

ne

se 
n REDUNDANCY

ae
RESOLUTION

p ;Rd d
q
p_ ; !
d d POS & ORIENT a INVERSE  q_
p ; !_
d d
CONTROL DYNAMICS
MANIPULATOR

p ;R
e e

p_ ; !
e e
DIRECT
KINEMATICS

Figure 2.2. Inverse dynamics control with redundancy resolution.

has been exploited.

Choosing
 
n = (I ? J yJ ) _ ? J_ yJ ( ? q_ ) + B ?1(K nen + Cen) (2.89)
where K n is a positive definite matrix, and folding it into (2.87) gives

V_ = 21 eTn (B_ ? 2C )en ? eTn K nen = ?eTn K nen  0 (2.90)

thanks to (2.88) and the skew-symmetry of the matrix B _ ? 2C . Therefore, it

can be concluded that the choice (2.89) gives a negative definite V_ , and thus
en ! 0 asymptotically.
Regarding the utilization of redundancy, a typical choice for  is
1 @w(q )
 
 = k B ?
@q (2.91)

where k is a signed scalar and w(q ) is an additional task function that can be
locally optimized.
A block diagram summarizing the overall inverse dynamics control with
redundancy resolution is sketched in Fig. 2.2. The inverse dynamics control
 
law gives as in (2.22) with as in (2.78). The position and orientation control
a
gives the resolved acceleration as in (2.29) with p as in (2.33) and o as a a
any of (2.40), (2.48), (2.57), (2.59), (2.72). The direct kinematics gives the
actual end-effector position and orientation as in (2.2) and (2.3), and the linear
and angular velocity as in (2.4) and (2.5). Finally, the redundancy resolution
 
scheme gives n as in (2.89) with as in (2.91). Obviously, if no redundancy

is available, then n = 0 and the inverse dynamics control law (2.26) shall be
used in lieu of (2.78).
26 ROBOT FORCE CONTROL

3. REGULATION
For certain motion tasks, it can be sufficient to guarantee regulation of the
p R
end effector to a constant desired position d and orientation d . In such a
case, simpler control laws can be adopted which do not require a full knowledge
of the manipulator dynamic model, but just a static model-based compensation.

3.1 STATIC MODEL-BASED COMPENSATION

Mechanical intuition suggests that task space regulation can be achieved
by designing a suitable control action which realizes an equivalent force and

moment aimed at driving the end effector toward the desired position and
orientation, i.e.
 J q K q gq
= T ( ) ? D _ + ( ), (2.92)
J
where the matrix T is needed to compute the resulting joint torques in a
g
way analogous to (2.18) and the term is needed to compensate for the static
K q K
torques due to gravity; also in (2.92), the derivative term ? D _ , with D an
(n  n) positive definite matrix gain, provides additional damping torques at
the joints which are expected to improve the transient behavior of the system.
The control based on (2.92) is known as proportional–derivative (PD) control
with gravity compensation.
Substituting (2.92) into (2.20) gives the closed-loop dynamic behavior of the
system in the form

B(q)q + C (q; q_ )q_ + (F + K D )q_ = J T(q) . (2.93)

The stability of this system can be studied by introducing the following positive
definite Lyapunov function candidate

V = 21 q_ TB (q)q_ + Up + Uo , (2.94)

where the first term is the kinetic energy of the manipulator while the second
and third terms are the potential energy associated respectively with a position
error and an orientation error to be defined afterwards.
h
In view of the partition of in (2.19), it is appropriate to partition the

vector into its force and moment components, i.e.

= po
 
(2.95)

where p and o are (3  1) vectors which shall be chosen so as to provide a
position and an orientation control action, respectively.
The desired end-effector position is specified in terms of the constant position
p
vector d , whereas the desired end-effector orientation is specified in terms of
R
the constant rotation matrix d .
 Motion Control 27

The position control can be chosen as a proportional action

p = K Pp
pde (2.96)

where
pde is defined in (2.32) and K Pp is a suitable feedback matrix gain.

Accordingly, the potential energy in (2.94) is

Up = 12
pTdeK Pp
pde . (2.97)

As regards the orientation control, this will also be chosen as a proportional

action, but the actual expression will depend on the particular definition of
end-effector orientation error chosen among those illustrated in the previous
section.

3.2 ORIENTATION ERRORS

The natural counterpart of the position control in (2.96) is obtained when
using the Euler angles error in (2.35) which leads to
o = T ?T('e)K Po
'de (2.98)

K
where Po is a suitable feedback matrix gain and T is defined in (2.36).
Accordingly, the potential energy in (2.94) is

Uo = 21
'TdeK Po
'de . (2.99)

In view of (2.97) and (2.99), the time derivative of (2.94) along the trajectories
of system (2.93) with (2.96) and (2.98) is

V_ = ?q_ T(F + K D )q_ , (2.100)

where (2.5) has been used together with the skew-symmetry of _ ? 2 . B C
Since V_ is only negative semi-definite, in view of LaSalle theorem, the system
asymptotically converges to the invariant set described by the equilibrium:
E = f
pde = 0;
'de = 0; q_ = 0g (2.101)

which is stable as long as no (kinematic or representation) singularity occurs.

'
For the alternative Euler angles error de that can be extracted from (2.42),
the orientation control action is
o = T ?e T('de)K Po'de (2.102)

where T e is defined in (2.47). The stability of the equilibrium

E = f
pde = 0; 'de = 0; q_ = 0g (2.103)
28 ROBOT FORCE CONTROL

can be proven in a formally analogous way to the above, by choosing the

potential energy in (2.94) as

Uo = 12 'TdeK Po'de. (2.104)

o
For the angle/axis error 0de that can be computed as in (2.53), the orientation
control action is
K o
o = Po de
0 (2.105)
with K Po = kPoI . The stability of the equilibrium
E = f
pde = 0; de = 1; ede = 0; q_ = 0g (2.106)

can be proven in a formally analogous way to the above, by choosing the

potential energy in (2.94) as

Uo = 2kPoeTdeede, (2.107)

while the equilibrium analysis is conceptually the same as in Subsection 2.3.


For the quaternion error e de that can be extracted from (2.42), the orienta-
tion control action is
K R 
o = Po e de
e (2.108)
K I
with Po = kPo . The stability of the equilibrium E in (2.106) can be proven
in a formally analogous way to the above, by choosing the potential energy
in (2.94) as  
 
Uo = kPo (de ? 1)2 + e Tde e de , (2.109)
while the equilibrium analysis is conceptually the same as in Subsection 2.4.

4. FURTHER READING
Kinematic and dynamic modeling of rigid robot manipulators can be found in
any classical robotics textbook, e.g. [81, 32, 38, 99, 116, 88, 6]. Identification
of dynamic parameters needed by an inverse dynamics control was studied
in [9, 58, 85]; the computation of a minimal set of dynamic parameters was
considered in [45], while the dynamic model identified for the robot manipulator
used in the experiment is reported in [16]. A complete treatment of model-
based control is provided in [1], whereas advanced control schemes are covered
in [23].
The original resolved acceleration control scheme dates back to [66] where
an angle/axis error was used. Subsequent developments of the scheme are
based on the work in [60]. The use of a quaternion error was proposed in [117]
where a nonlinear closed-loop system for the orientation error was obtained; a
linear closed-loop system was instead obtained in [59] at the expense of a more
 Motion Control 29

involved choice of resolved angular acceleration. Further material about rigid

body orientation can be found in classical mechanics textbook, e.g. [47, 86]
as well as in an advanced robotics textbook [70]; a treatment of quaternion
algebra is included in [30] while an algorithm for the extraction of a quaternion
from a rotation matrix was given in [90]. The general problem of attitude
control using quaternions was addressed in [107, 39]. Other representations
of orientation for resolved acceleration control are discussed in [106], while
a complete reformulation of resolved acceleration control based on different
types of orientation errors was presented in [18], including the alternative Euler
angles error which mitigates the effects of representation singularities.
A large body of literature is available on kinematic redundancy. The main
techniques developed for redundant manipulators are described in [71]. The
problem of solving redundancy at the acceleration level is discussed in [34].
The issue of stabilization of the internal motion was focused in [53]. The
properties of the dynamically consistent pseudo-inverse of the manipulator
Jacobian are analyzed in [43].
The PD control with gravity compensation has long constituted the simplest
position control strategy for a robot manipulator. Its stability was keenly proven
in the seminal work [100] where the passivity property of the dynamic model
is fully exploited.
Chapter 3

INDIRECT FORCE CONTROL

The potential offered by motion control strategies to ensure a compliant behavior during
the interaction between the end effector and the environment is investigated. Compli-
ance control and impedance control are introduced as effective tools to achieve indirect
control of the contact force. An inner motion loop is adopted to enhance disturbance
rejection. Six-DOF impedance control schemes are derived using different types of ori-
entation displacement and the properties of rotational stiffness are analyzed in detail. A
generalization to the case of nondiagonal six-DOF stiffness is discussed. Throughout the
chapter, experimental results are presented for an industrial robot interacting with the
environment.

1. COMPLIANCE CONTROL
In order to gain insight into the problems arising at the interaction between
the end effector and the environment, it is worth analyzing first the effects of
a motion control strategy in the presence of a contact force and moment. In
the previous chapter, it has been shown that a static model-based compensation
control provides regulation to a desired end-effector position and orientation,
while a dynamic model-based compensation control is needed to ensure ac-
curate tracking of a desired end-effector trajectory. The two control schemes
are reconsidered at the beginning of this chapter as useful tools to achieve an
indirect force control, i.e. a control of the force (moment) through suitable
actions on the position (orientation) error at the end effector.
For the sake of illustrating the useful concepts of compliance and impedance,
the end-effector position and orientation is described by a (6  1) vector e = x
p '
[ Te Te ]T , where the classical Euler angles representation of orientation has
been selected in spite of its potential drawbacks concerned with representation
singularities. Accordingly, the desired position and orientation is denoted as d x
with obvious meaning of its components. As a consequence, the end-effector
31
32 ROBOT FORCE CONTROL

error can be denoted as

pde
 

xde =
'de . (3.1)

It should be noticed that any of the other representations of orientation consid-

ered in the previous chapter leads to a (3  1) orientation error that can be used
'
in lieu of
de in (3.1).

1.1 ACTIVE COMPLIANCE

h
In the case of a nonnull contact force and moment ( 6= 0), the static model-
based compensation control scheme in Section 3. of the previous chapter no
longer ensures that the end effector reaches its desired position and orientation.
In fact, by using the error in (3.1) and accounting for (2.96) and (2.98), the

vector in (2.92) becomes

= H ?T('e)K P
xde (3.2)

where
K P = KOPp KOPo .
 
(3.3)

and
I O
H= O T , (3.4)

with T
in (2.36) and O denoting the (3  3) null matrix. The steady state
q q
( _ = 0;  = 0) of the system (2.20) with (2.92) and (3.2) is

J TH ?T('e)K P
xde = J Th. (3.5)

On the assumption of a full-rank Jacobian, Equation (3.5) gives

xde = K ?P 1 H T ('e )h. (3.6)

Equation (3.6) shows that at steady state the manipulator, under a proportional
control action on the position and orientation error, behaves as a generalized
h
spring in respect of the force and moment . Thus, the matrix ? K 1
P plays the
role of an active compliance, meaning that it is possible to act on the elements
K
of P so as to ensure a compliant behavior during the interaction.
K
The selection of the elements of P is not an easy task, since it is strongly
affected by the characteristics of the environment. For a better understanding of
interaction between the end effector and the environment it would be necessary
to have an analytical description of the contact force and moment, which would
be very demanding from a modeling viewpoint.
 Indirect Force Control 33

On the other hand, the partitions of K P in (3.3),
xde in (3.1) and h

in (2.19), as well as the expression of H in (3.4), suggest separating Equa-
tion (3.6) into

pde = K ?Pp1 f (3.7)

'de = K ?Po1 T T ('e ) (3.8)

which allow analyzing the compliant behavior of the end-effector position

separately from that of the end-effector orientation.
T
The presence of the matrix in (3.8) reveals that, even on the assumption
of knowing the contact moment, the actual compliance for the end-effector
K
orientation does depend not only on the choice of Po but also on the choice
of the particular set of Euler angles and in turn on the current manipulator
configuration through the kinematics equation (2.3). Furthermore, the use of
different orientation errors may bear noticeable implications as concerns the
active compliance for the rotational part, and thus the analysis is deferred to
Section 3.
Nonetheless, at this stage, it is worth analyzing the behavior for the transla-
tional part to acquire familiarity about how to act on the compliance for a given
model of contact force. A real contact is a naturally distributed phenomenon
in which the local characteristics of both the end effector and the environment
are involved. In addition, friction effects between parts typically exist which
greatly complicate the nature of the contact itself. A simplified analysis can
be pursued by considering a frictionless and elastically compliant environment
which is described by the model

f = K f (pe ? po ) (3.9)

p p
where e is the end-effector position at the contact point, while o is the rest
K
position and f is the (3  3) contact stiffness matrix of the environment.
With the environment model (3.9), Equation (3.7) becomes

pde = K ?Pp1 K f (pe ? po ); (3.10)

thus, at steady state, the end-effector position is given by

 ?1  
pe;1 = I + K ?Pp1K f pd + K ?Pp1K f po , (3.11)

while the contact force can be found to be

 ?1
f 1 = I + K f K ?Pp1 K f (pd ? po ). (3.12)

Analysis of (3.11) shows that the steady-state position depends on the environ-
ment rest position as well as on the desired position imposed by the control
34 ROBOT FORCE CONTROL

system to the end effector. The interaction of the two systems (environment and
end effector) is influenced by the mutual weight of the respective compliance
features. It is then possible to decrease the active compliance so that the end
effector dominates the environment and vice versa. Such a dominance can be
specified with reference to the single task directions, assuming that both Pp K
K
and f are diagonal. For a given contact stiffness, according to the prescribed
K
interaction task, large values of the elements of Pp are to be chosen for those
directions along which the environment has to comply, whereas small values
K
of the elements of Pp are to be chosen for those directions along which the
end effector has to comply.
Expression (3.12) gives the value of the contact force at steady state, which
reveals that it may be appropriate to tune the end-effector compliance with
the environment compliance along certain task directions. In fact, along a
direction with high contact stiffness, it is better to have a compliant end effector
so that it can taper the intensity of interaction through a suitable choice of
the desired position. In this case, the end-effector steady-state position e;1p
p
practically coincides with the environment undeformed position o . Hence, the
end effector sustains the elastic force, depending on the corresponding elements
K p p
of P , that is determined by the choice of the component of ( d ? o ) along
the relative direction. In the dual case of high environment compliance, if the
p
end effector is made stiff, the steady-state position e;1 is very close to the
p
desired position d . Hence, it is the environment to sustain the elastic force
along the constrained task directions of interest.
In certain cases, it is possible to employ mechanical devices interposed
between the manipulator end effector and the environment so as to change the
passive compliance along particular task directions. For instance, in a peg-
in-hole insertion task with a tight clearance, the peg axis would be required
to line up almost exactly with the hole axis. If a discrepancy exists between
the peg and the hole axes in either distance or direction, then the insertion
cannot be accomplished, because of jamming or wedging. To accommodate
the insertion, the gripper is provided with a device ensuring high stiffness
along the insertion direction and high compliance along the other directions
(remote center of compliance). The inconvenience of such devices is their low
versatility to different operating conditions and generic interaction tasks, i.e.
whenever a modification of the compliant mechanical hardware is required.
On the other hand, with active compliance actions the control software can be
easily modified so as to satisfy the requirements of different interaction tasks.

1.2 EXPERIMENTS
The above compliance control scheme has been tested in experiments on the
six-joint industrial robot with open control architecture described in Section 3.
of Chapter 1. The force sensor is not used for control but only for measurement
 Indirect Force Control 35

Ob

Xb

Yb

Zb

q1
q2
q3
q4
pe

ne

se

ae

p ;R
d d

p_ ; !
d d

p ; !_
d d

a
 n


q
q_
p ;R
e e

p_ ; !
e e

Figure 3.1. Robot end effector in contact with cardboard box.

of contact forces. The kinematic model and dynamic model of the robot
manipulator are given in Appendix B.
Only the inner three joints are used while the outer three joints are mechani-
J J
cally braked. Hence n = 3 in (2.92) with = p and = p . Accordingly, a
three-DOF task is considered, involving end-effector position and linear force.
The environment is constituted by a cardboard box, where the stiffness depends
on the contact point and is about 104 N/m.
An end effector has been built as a stick with a sphere at the tip, both made
of steel. A picture illustrating the robot with the wrist force sensor and the
environment is given in Fig. 3.1.

Case Study. With reference to the base frame, the task consists of a straight-
line motion in the Yb Zb -plane with an end-effector (horizontal) displacement
of 0:25 m along Yb and (vertical) displacement of ?0:15 m along Zb . The
trajectory along the path is generated according to a trapezoidal velocity profile
with cubic blends, and null initial and final velocities and accelerations, and a
duration of 6 s. The surface of the cardboard box is nearly flat and has been
placed (horizontally) in the Xb Yb -plane in such a way as to obstruct the desired
end-effector motion. The gains of the control action in (2.96) and (2.92) have
K I K I
been set to Pp = 8000 and D = 20 ; note that Pp has been chosen K
on the basis of a trade-off between position accuracy during the unconstrained
motion and compliant behavior at the end effector (limited values of contact
 bO
bX
bY
bZ
q136 ROBOT FORCE CONTROL
q2
q3
q4 end−effector path contact force
ep 150
en 0 z
es
ea 100
p ;R
d d −0.05
p_ ; !
z [m]

[N]
d d

p ; !_ 50
d d

a −0.1

n
0
x

q −0.15 y
q_ −50
p ;R
e e 0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
p_ ; !
e e y [m] time [s]

Figure 3.2. Experimental results under compliance control.

K
force) during the constrained motion, while D has been chosen so as to
guarantee a well-damped behavior.
The results are presented in Fig. 3.2 in terms of the desired (dashed) and the
actual (solid) end-effector path, together with the time history of the contact
force; in order to facilitate interpretation of the results, the approximate location
(dotted) of the surface is illustrated on the plot of the end-effector path, while
the instant of contact (dotted line) and the instant of the end of the motion
trajectory (dashed line) are evidenced on the plot of the contact force.
It can be recognized that path tracking accuracy is rather poor during execu-
tion of the whole task. On the other hand, the contact force along Zb reaches a
steady-state value but its amount is rather large. Reduction of the contact force
K
could be obtained by decreasing Pp , at the expense of a larger end-effector
position error though.
Finally, notice the presence of an appreciable value of contact force along
Yb at steady state due to contact friction, which indeed has not been modeled
in the above analysis.

2. IMPEDANCE CONTROL
Compliance control is designed to achieve a desired static behavior of the
interaction. In order to achieve a desired dynamic behavior, the actual mass
and damping at the contact are to be considered besides the stiffness, leading
to impedance control.

2.1 ACTIVE IMPEDANCE

In order to further investigate the interaction between the end effector and
the environment, it is worth analyzing the performance of the dynamic model-
based compensation control scheme in Section 2. of the previous chapter. The
inverse dynamic control law (2.22) in the case of a nonnull contact force and
 Indirect Force Control 37

moment ( h 6= 0) leads to
q =  ? B?1 (q)J T(q)h (3.13)

in lieu of (2.23). Equation (3.13) reveals the existence of a nonlinear coupling


term due to the contact force and moment. The choice of as in (2.26) with
a a
p as in (2.33) and o as in (2.40) leads to

xde + K D
x_ de + K P
xde = H ?1 ('e )J (q )B ?1 (q )J T (q )h (3.14)

x
with
de in (3.1), K P in (3.3), H in (3.4) and

K
K D = O K Do .
Dp O 
(3.15)

Equation (3.14) establishes a relationship through a generalized active impe-

h
dance between the contact force and moment and the end-effector position
x
and orientation error
de . This impedance can be attributed to a mechanical
system characterized by a mass matrix

M = J ?TBJ ?1 H , (3.16)

a damping matrix MK D and a stiffness matrix MK P , which allows speci-

fying the dynamic behavior of the end effector.
The presence of M makes the system coupled and nonlinear. If it is wished
to keep linearity and decoupling during the interaction with the environment, it
is necessary to measure the contact force and moment; this can be achieved by
means of an appropriate force/torque sensor which is usually mounted at the
manipulator wrist. Choosing

 = B(q) + C (q; q_ )q_ + F q_ + g(q) + J T (q)h (3.17)

with  as in (2.26),
ap = p d + K ?Mp1 (K Dp
p_ de + K Pp
pde ? f ) (3.18)

ao = T ('e)('d + K ?Mo1 (K Do
'_ de + K Po
'de ? T T('e))) (3.19)

+T_ ('e; '_ e )'_ e
with K Mp and K Mo positive definite matrix gains, leads to

K Mp
pde + K Dp
p_ de + K Pp
pde = f (3.20)

K Mo
'de + K Do
'_ de + K Po
'de = T ('e) (3.21)
T

on the assumption of error-free force measurements.

 s e

a e

p ;R
d d

p_ ; !
d d

p ; !_
d d

a
 n


q
q_
38
p ; ROBOT
e R e
FORCE CONTROL
p_ ; !
e e

p ;R d d f; 
p_ ; ! IMPEDANCE a
d d INVERSE  MANIPULATOR q
p ; !_
d d
CONTROL DYNAMICS & ENVIRONMENT q_

p ;R
e e

p_ ; !
e e
DIRECT
KINEMATICS

Figure 3.3. Impedance control.

It is worth noticing that the addition of the term T in (3.17) exactly J h

compensates the contact force and moment and then it renders the end effector
infinitely stiff with respect to the environment. In order to confer a compliant
f
behavior at the end effector, the terms in (3.18) and T in (3.19) have T 
been introduced which allow characterizing the end effector as a linear and
decoupled impedance for both the translational part in (3.20) with regard to the
f
contact force and the rotational part in (3.21) with regard to the equivalent
contact moment T . T 
The behavior of the system described by (3.20) and (3.21) at steady state is
analogous to that described by (3.7) and (3.8); hence, the need for the matrix T T
in (3.19); nonetheless, compared to the active compliance specified by ? 1
P , K
now Equations (3.20) and (3.21) allow specifying the end-effector dynamics
through a six-degree-of-freedom (six-DOF) impedance, where the translational
K K K
impedance is specified by Mp , Dp and Pp , and the rotational impedance
K
is specified by Mo , Do and Po .K K
As anticipated with the treatment of active compliance, the use of different
orientation errors shall be studied in detail as concerns the rotational impedance.
A block diagram of the resulting impedance control is sketched in Fig. 3.3.
With respect to the inverse dynamics scheme for motion control in Fig. 2.2, the
position and orientation control is replaced with an impedance control which
handles the measured contact force and moment and provides the resolved
acceleration as in (3.18) and (3.19). Then, the inverse dynamics control law
 
gives as in (3.17) with as in (2.26). Redundancy resolution can be merely

added to the scheme in Fig. 3.3 if wished, since the action of n is not influenced
by the contact force and moment in view of the choice of y as in (2.83). J
2.2 INNER MOTION CONTROL
The selection of good impedance parameters that guarantee a satisfactory
compliant behavior during the interaction may turn out to be inadequate to en-
p_ ; !
d d

p ; !_
d d

a

f; 
q
q_
p ;R
e e

p_ ; !
e e

Indirect Force Control 39

p ;R
d d p ;R
c c f; 
p_ ; !
d d IMPEDANCE p_ ; !
c c POS & ORIENT a INVERSE  MANIPULATOR q
p ; !_
d d
CONTROL p ; !_
c c
CONTROL DYNAMICS & ENVIRONMENT q_

p ;R
e e

p_ ; !
e e
DIRECT
KINEMATICS

Figure 3.4. Impedance control with inner motion control loop.

sure accurate tracking of the desired position and orientation trajectory when the
end effector moves in free space. In fact, by stacking the two equations (3.20)
and (3.21) and accounting for the presence of a disturbance as in (2.28) yields
K M
xde + K D
x_ de + K P
xde = H T('e)h + K M H ?1('e)d (3.22)
with
K
K M = O K Mo .Mp O 
(3.23)

An effective disturbance rejection, at least at steady state, can be achieved by

choosing a low weight for the matrix K ? P K M which corresponds to a stiff
1

control action with an equivalent light mass at the end effector. Such a feature
can eventually conflict with the desire of guaranteeing a compliant behavior
when the end effector is in contact with a rather stiff environment.
A solution to this drawback can be devised by separating the motion control
action from the impedance control action as follows. The motion control
action is purposefully made stiff so as enhance disturbance rejection but, rather
than ensuring tracking of the desired end-effector position and orientation, it
shall ensure tracking of a reference position and orientation resulting from the
impedance control action. In other words, the desired position and orientation
together with the measured contact force and moment are input to the impedance
equation which, via a suitable integration, generates the position and orientation
to be used as a reference for the motion control action.
In order to realize the above solution, it is worth introducing a reference
frame other than the desired frame specified by d and d . This frame is p R
referred to as the compliant frame c , and is specified by a position vector c p
R
and a rotation matrix c . In this way, the inverse dynamics motion control
strategy can be still adopted as long as the actual end-effector position e p
R
and orientation e is taken to coincide with c and c in lieu of d and d ,p R p R
respectively. Accordingly, the actual end-effector linear velocity _ e and angular p
! p
velocity e are taken to coincide with _ c and c , respectively. !
40 ROBOT FORCE CONTROL

A block diagram of the resulting scheme is sketched in Fig. 3.4 and reveals
the presence of an inner motion control loop with respect to the outer impedance
control loop.
The remainder of this chapter is devoted to analyze in detail the realization of
an active impedance between the desired frame d and the compliant frame c
for the translational part and the rotational part in lieu of the respective equa-
tions (3.20) and (3.21). The former is considered next leading to a three-DOF
impedance control scheme.

2.3 THREE-DOF IMPEDANCE CONTROL

The mutual position between d and c can be described by the position
displacement

pdc = pd ? pc (3.24)

which has been referred to the base frame.

In view (3.20), the translational impedance equation is chosen so as to en-
force an equivalent mass-damper-spring behavior for the position displacement
f
when the end effector exerts a force on the environment, i.e.

M p
pdc + Dp
p_ dc + K p
pdc = f ; (3.25)

where M p , D p and K p are positive definite matrices.

With reference to the scheme in Fig. 3.4, the three-DOF impedance control
generates the reference position for the inner motion control. Therefore, in
order to allow the implementation of the complete control scheme, the linear
acceleration to be used in lieu of (3.18) shall be designed to track the position
and the linear velocity of c , i.e.

ap = p c + K Dp
p_ ce + K Pp
pce (3.26)

where

pce = pc ? pe (3.27)

is the position error between c and e . Notice that pc and its associated
derivatives can be computed by forward integration of the translational impe-
f
dance equation (3.25) with input available from the force/torque sensor.

2.4 EXPERIMENTS
The above three-DOF impedance control schemes have been tested in ex-
periments on the six-joint industrial robot where the first three joints are used,
J J h f
i.e. n = 3 in (3.17) with = p and = . Now, the force sensor is used for
control. The environment is the same as described in Section 1.2.
p ;R
d d

p_ ; !
d d

p ; !_
d d

a
n


q
q_
p ;R
e e

p_ ; !
e e

p ;R
d d

p_ ; !
d d

p ; !_
d d

a

f;  Indirect Force Control 41
q
q_
p ;R
e e
end−effector path contact force
p_ ; !
e e 40
p ;R
d d
0
p_ ; !
d d 30
p ; !_ z
d d

p ;R
c c −0.05 20
p_ ; !
z [m]

[N]
c c

p ; !_
c c

a −0.1 10
x

f;  0
q −0.15
y
q_ −10
p ;R
e e 0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
p_ ; !
e e y [m] time [s]

Figure 3.5. Experimental results under three-DOF impedance control.

First case study: Impedance control. The end effector has been placed in
the same initial position as for the previous case study with the same trajectory.
The impedance parameters in (3.18) have been set to Mp = 10 , Dp = K IK
I K I
255 and Pp = 500 , where the choice of Pp is aimed at obtaining a value K
of the contact force along Zb of approximately 20 N with the available estimate
of the surface stiffness.
The results are presented in Fig. 3.5 in terms of the desired (dashed) and the
actual (solid) end-effector path, together with the time history of the contact
force. As above, the approximate location (dotted) of the surface is illustrated
on the plot of the end-effector path, while the instant of contact (dotted line)
and the instant of the end of the motion trajectory (dashed line) are evidenced
on the plot of the contact force.
It can be recognized that path tracking accuracy is poor during execution
of the whole task; this is imputable to the disturbance term on the right-hand
side of (3.22). On the other hand, the contact force along Zb is limited during
the transient and reaches a constant value at steady state. Improvement of the
position tracking accuracy might be achieved by increasing Pp ; however, K
this would give rise to larger contact forces. Finally, notice the presence of an
appreciable value of contact friction force along both Xb and Yb at steady state
which is caused by the end-effector position deviation along both Xb and Yb
(although the former is not visible in the figure).

Second case study: Impedance control with inner motion control. In

order to carry out a fair comparison with the previous control scheme, the
impedance parameters in (3.25) have been set to the same values as in the
previous case study, i.e. M I D
p = 10 , p = 255 and p = 500 , while I K I
the inner position loop gains in (3.26) have been set to Dp = 90 and K I
K Pp = 2500 . I
 b O
bX
b Y
b Z
q1
q2
q3
q4
e p
e n
e s
e a 42 ROBOT FORCE CONTROL
p ;R
d d

p_ ; !
d d

p ; !_
d d end−effector path contact force
a 40
n 0
 30
q
q_ −0.05 z
p ;R 20
z [m]

[N]
e e

p_ ; !
e e

p ;R
d d −0.1 10
p_ ; !
d d
x
p ; !_
d d 0
a −0.15 y
 −10
f;  0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
q y [m] time [s]
q_
p ;R
e e
end−effector path contact force
p_ ; !
e e
40
p ;R
d d 0 z
p_ ; !
d d 30
p ; !_
d d

p ;R
c c −0.05 20
p_ ; !
z [m]

[N]

c c

p ; !_
c c
10
a −0.1
 x
f;  0
q −0.15
y
q_ −10
p ;R
e e 0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
p_ ; !
e e
y [m] time [s]

Figure 3.6. Experimental results under three-DOF impedance control with inner motion con-
trol.

The results are presented in the upper part of Fig. 3.6 in terms of the desired
(dashed) and the actual (solid) end-effector path, together with the time history
of the contact force. As above, the approximate location (dotted) of the surface
is illustrated on the plot of the end-effector path, while the instant of contact
(dotted line) and the instant of the end of the motion trajectory (dashed line)
are evidenced on the plot of the contact force.
It can be recognized that path tracking accuracy is noticeably improved with
respect to that obtained with the previous scheme and now is very good; this
confirms the effective rejection of the disturbance thanks to the inner position
loop.
On the other hand, the contact force along Zb is still limited during the
transient and reaches an approximate value of 20 N at steady state, as wished
K
with the choice of p above. As before, an appreciable value of contact
friction force along Yb occurs that remains at steady state, while the good end-
effector tracking accuracy essentially causes no contact friction along Xb by
maintaining the motion in the Yb Zb -plane.
 Indirect Force Control 43

To investigate robustness of the scheme with respect to changes in the envi-

ronment location, the task has been repeated with the same impedance parame-
ters and inner position loop gains as before but the cardboard box was raised by
about 0:025 m. From the results presented in the lower part of Fig. 3.6, it can be
recognized that the imposed motion would require the end effector to penetrate
K
into the surface by a larger amount, and thus the same value of p gives rise to
a different (larger in this case) contact force at steady state. It is worth noticing
that the larger value of contact force yields larger contact friction as well.

3. SIX-DOF IMPEDANCE CONTROL

p
Equation (3.25) admits a nice physical interpretation when d is constant,
M p = mpI and K p is symmetric. In this case, the first term represents the
inertial force acting on a rigid body with mass mp and kinetic energy

Tp = 12 mpp_ Tc p_ c: (3.28)

The second term represents a dissipative damping force, while the last term
represents the force exerted on the body by a three-dimensional spring with
K p
stiffness matrix p , equilibrium position d and potential energy

Up = 12
pTdcK p
pdc: (3.29)

In order to ensure a proper end-effector behavior for the successful execution

of an interaction task, the selection of the stiffness matrix plays a key role.
Therefore, it is worth analyzing the elastic term from a geometric point of view.
K
The stiffness matrix p can be decomposed as

K p = U p?pU Tp ; (3.30)

where ?p = diagf p1 ; p2 ; p3 g and U p = [ up1 up2 up3 ] are respectively

the eigenvalue matrix and the (orthogonal) eigenvector matrix. Then, consid-
ering a position displacement of length  along the i-th eigenvector leads to an
elastic force
f K p
E = p
dc = pi  pi u (3.31)
which represents an elastic force along the same upi axis. This implies that the
translational stiffness matrix can be expressed in terms of three parameters pi
representing the stiffness along three principal axes u pi , and in turn it establishes
the property of task geometric consistency for the elastic force in (3.31).
As far as the rotational impedance is concerned, the analysis depends on the
particular type of representation for the orientation displacement between d
and c . In the following subsections, the rotational impedance equation is
44 ROBOT FORCE CONTROL

derived for the Euler angles displacement, the angle/axis displacement and
the quaternion displacement, already introduced with reference to the motion
control problem to express an end-effector orientation error.

3.1 EULER ANGLES DISPLACEMENT

In view of (3.21), the rotational impedance equation based on the classical
Euler angles displacement is

M o
'dc + Do
'_ dc + K o
'dc = T T('c), (3.32)

where M o , D o and K o are positive definite matrices describing the generalized

inertia, rotational damping, rotational stiffness, respectively, and

'dc = 'd ? 'c. (3.33)

Notice that, differently from (3.25), the dynamic behavior for the rotational
part is not absolutely determined by the choice of the impedance parameters but
it does also depend on the orientation of the compliant frame with respect to the
T '
base frame through the matrix T ( c ). Moreover, Equation (3.32) becomes
ill-defined in the neighborhood of a representation singularity; in particular, at
T
such a singularity, moment components in the null space of T do not generate
any contribution to the dynamics of the orientation displacement, leading to a
possible build-up of large values of contact moment.
The effect of the rotational stiffness can be better understood by considering
an infinitesimal orientation displacement between d and c . From (3.32), in
the absence of representation singularities, the elastic moment is

E = T ?T('c)K o
'dc . (3.34)

In the case of an infinitesimal orientation displacement about 'c, it is


d(
'dc ) = ('_ d ? '_ c) ' = ' dt (3.35)
d c
= T ('c )
!dc dt,
? 1

! ! !
where
dc = d ? c is the relative angular velocity between the two frames.
Folding (3.35) into (3.34) written for an infinitesimal displacement d(
dc ) '
gives
E= T ' KT ' !
?T ( ) o ?1 ( )
dc dt.
c c (3.36)
Equation (3.36) reveals that the relationship between the orientation displace-
ment and the elastic moment depends on the orientation of c . It follows that
the property of task geometric consistency of the elastic force (3.31) is lost
 Indirect Force Control 45

when considering the elastic moment (3.34), that is, the eigenvectors of the
K
matrix o do not represent the three principal axes for the rotational stiffness.
The drawbacks discussed above can be mitigated by adopting the alternative
'
Euler angles displacement dc that can be extracted from the rotation matrix
cR
d = Rc R d .
T
(3.37)

Then, a rotational impedance at the end effector can be defined as

M o ' dc + Do'_ dc + K o'dc = T T('dc)c (3.38)

where M o , D o and K o are defined in a similar way to (3.32) and c  is referred

to c .
An advantage with respect to (3.32) is that now the impedance behavior
for the rotational part depends only on the relative orientation between d
T '
and c through the matrix T ( dc ). Hence, if XYZ Euler angles are adopted,
representation singularities have a mitigated effect since they occur when dc =
=2, i.e. for large end-effector orientation displacements.
From (3.38) the elastic moment is
c = T ?T ('dc )K o 'dc .
E (3.39)

The infinitesimal orientation displacement about 'dc = 0 is


d'dc = '_ dc 'dc = 0 dt

 (3.40)

= T ?1 (0)
c !dc dt.

'
Folding (3.40) into (3.39) written for an infinitesimal displacement d dc gives
c
E =T
?T (d' )K o T ?1 (0)
c! dc dt
dc (3.41)
' T ?T(0)K oT ?1 (0)
c!dcdt
T ' T
where the first-order approximation ?T (d dc )dt ' ?T (0)dt has been
made. Equation (3.41) reveals that the relationship between the orientation
displacement and the elastic moment is independent of the orientation of c .
Notice, however, that the choice of Euler angles affects the resulting stiffness
T
through the matrix (0) which must be invertible; as already emphasized in
Subsection 2.2 of the previous chapter, the widely-adopted ZYZ representation
'
of Euler angles cannot be used here, being singular right at dc = 0 (see the
matrix in (A.18))! It is convenient, instead, to adopt the XYZ representation
T I
which gives (0) = (see the matrix in (A.17)) and thus, for an infinitesimal
displacement,
E
c ' o
c dt. K !dc (3.42)
46 ROBOT FORCE CONTROL

As regards the property of task geometric consistency for the elastic moment
K
(3.39), when o is a diagonal matrix and the XYZ representation of Euler
u K
angles is adopted, the i-th eigenvector oi of o = diagf o1 ; o2 ; o3 g is the
i-th column of the identity matrix. Hence, the orientation displacement of an
u
angle #dc about oi is described by
'dc = #dcuoi (3.43)
which, in view of the expression of T ('dc ) for XYZ Euler angles, leads to
c = # u ,
E oi dc oi (3.44)
representing an elastic moment about the same uoi axis; thus the vectors uoi
have the meaning of rotational stiffness principal axes. It can be easily recog-
nized that the same property does not hold in general for a nondiagonal K o .
Equations (3.32) or (3.38) define the dynamic behavior between d and c
in terms of a rotational impedance. Therefore, in order to allow the implemen-
tation of the complete six-DOF impedance control with inner motion loop in
a
Fig. 3.4, the angular acceleration o shall be designed to track the orientation
and angular velocity of c .
With reference to the Euler angles error used in (2.40), the angular acceler-
ation is taken as
ao = T ('e) ('c + K Do
'_ ce + K Po
'ce) + T_ ('e; '_ e)'_ e (3.45)
where

'ce = 'c ? 'e (3.46)
is the orientation error between c and e . Notice that 'c and its associated
derivatives can be computed by forward integration of the rotational impedance

equation (3.32) with input available from the force/torque sensor.
On the other hand, with reference to the alternative Euler angles error used
in (2.48), the angular acceleration is taken as
ao = !_ d ? T_ e('de; '_ de)'_ de (3.47)
?T e('de) ('dc + K Do('_ dc ? '_ de ) + K Po('dc ? 'de))
where the orientation control acts as to take 'de to coincide with 'dc which
ultimately implies that e is aligned with c . Notice that 'dc and its associated
derivatives can be computed by forward integration of the rotational impedance
equation (3.38).

3.2 ANGLE/AXIS DISPLACEMENT

A class of geometrically meaningful representations of the mutual orientation
between d and c can be given in terms of the angle/axis displacement
co
dc = f (#dc ) rdc ,
c (3.48)
 Indirect Force Control 47

r R
where #dc and c dc correspond to c d , and f (#dc ) is any of the functions
listed in Tab. 2.1. Those are strictly increasing smooth functions in an inter-
val (?#M ; #M ) with #M > 0. Hence, the derivative f 0 (#dc ) of f with respect
to #dc is strictly positive in that interval.
From (A.10) the angular velocity of d relative to c is given by

c! dc = c !d ? c !c = RTc (! d ? !c ). (3.49)

Differentiating (3.48), and using (A.26) and (A.27), gives

c o_
dc =
( rdc ; #dc )
! dc
c c (3.50)

where

=
k +
? (3.51)
with

k = f 0(#dc )c rdc c rTdc (3.52)
1  

? = f (#dc) cot(#dc =2)(I ? c rdc crTdc ) ? S (c rdc ) . (3.53)
2
The matrix
k (
? ) projects the relative angular velocity
c ! dc in a direction
parallel (orthogonal) to c odc . Also, notice that the following property of

holds

(c rdc ; 0) = f 0(0)I (3.54)
which will be useful in the following.
In order to derive the impedance equation for the rotational part, it is conve-
nient to refer to the following energy-based argument. Let

To = 12
c!TdcM o
c!dc (3.55)

express the rotational kinetic energy of a rigid body with inertia tensor o M
!
and angular velocity
c dc . It is worth pointing out that, from a rigorous
physical viewpoint, To is representative of a pseudo-kinetic energy since it is
defined in terms of a relative velocity. Nonetheless, it should be clear that, if
the orientation of d is constant, then it would attain the meaning of a true
kinetic energy. Then consider the potential energy

Uo = coTdcK ocodc, (3.56)

where is a positive constant depending on the particular choice of f (
), and

K o is a symmetric positive definite matrix.
Having defined the various energy contributions, the terms in the rotational
impedance equation can be derived by considering the associated powers.
48 ROBOT FORCE CONTROL

Taking the time derivative of (3.55) yields

T_o = cTI
c!dc (3.57)

where
c = M o
c!_ dc
I (3.58)

! !
is the inertial moment and
c _ dc denotes the time derivative of
c dc in (3.49).
Further, taking the time derivative of (3.56) and accounting for (3.50) yields

U_o = cTE
c!dc (3.59)

where
c = 2
T (c rdc ; #dc )K o c odc
E (3.60)
is the elastic moment.
Finally, a dissipative contribution can be added as
c = Do
c! dc ,
D (3.61)

D
where o is a positive definite matrix characterizing a rotational damping at
the end effector.
Therefore, a rotational impedance at the end effector can be defined by
adding the contributions (3.58), (3.61) and (3.60), i.e.

M o
c!_ dc + Do
c!dc + K 0ocodc = c, (3.62)

where the equality c  = c I + c D + c E has been imposed, and

K 0o = 2
T(crdc; #dc)K o. (3.63)

Notice that the rotational part of the impedance equation has been derived in
terms of quantities all referred to c ; this allows the impedance behavior to be
effectively expressed in terms of the relative orientation between d and c ,
no matter what the absolute orientation of the compliant frame with respect to
the base frame is.
It is worth remarking that, by adopting an angle/axis representation of the
orientation and pursuing an energy-based argument, the contributions in the
rotational impedance equation correspond to physically meaningful energy
terms; also, the velocity used is dual to the moment 
exerted by the end
effector, i.e. with no need of a transformation matrix depending on the actual
end-effector orientation.
In the following, the analysis for small orientation displacements is carried
out and consistency with the task geometry is investigated.
 Indirect Force Control 49

Consider an infinitesimal orientation displacement expressed as


dcodc = c o_ dc # = 0 dt (3.64)
dc
=
(c rdc ; 0)
c ! dc dt
= f 0(0)
c ! dc dt
where the property (3.54) has been exploited. Folding (3.64) into (3.60), written
for an infinitesimal displacement about #dc = 0, gives
c
T (c rdc ; d#dc )K o dcodc
E =2 (3.65)
' 2 ?f 0(0)
2 K o
c!dcdt
= K o
c ! dc dt,
r I
where the first-order approximation
(c dc ; d#dc ) ' f 0 (0) has been con-
sidered and the choice = 1=2(f 0 (0))2 has been made. Equation (3.65)
clearly shows how the relationship between the orientation displacement and
the elastic moment is independent of the orientation of c , and the problem of
representation singularities is not of concern since f 0 (0) is finite.
As regards the property of task geometric consistency, the stiffness matrix
in (3.60) can be decomposed as

K o = U o?o U To (3.66)

where ?o = diagf o1 ; o2 ; o3 g and U o = [ uo1 uo2 uo3 ] are respectively

the eigenvalue matrix and the (orthogonal) eigenvector matrix. Then, consid-
ering an orientation displacement by an angle #dc about the i-th eigenvector

ocdc = f (#dc)uoi ; (3.67)

and taking into account the decomposition of

into the two terms (3.52)
and (3.53) yields
c = 2
Tk (uoi ; #dc ) oi f (#dc )uoi = 2 f 0 (#dc )f (#dc ) oi uoi .
E (3.68)

u
This represents an elastic moment about the same oi axis which is in the same
direction of the orientation displacement since f 0 (#dc ) > 0. Therefore, the
rotational stiffness matrix can be expressed in terms of three parameters oi
u
representing the stiffness about three principal axes oi , i.e. in a consistent way
with the task geometry.
For the implementation of the inner motion loop in Fig. 3.4, with reference
to the angle/axis error used in (2.59), the angular acceleration is taken as

ao = !_ c + K Do
!ce + K Poo0ce (3.69)

50 ROBOT FORCE CONTROL

where
o0ce = 21 (S (ne)nc + S (se)sc + S(ae)ac) (3.70)

where nc , sc , ac are the column of the matrix Rc which can be computed by

forward integration of the rotational impedance equation (3.62).

3.3 QUATERNION DISPLACEMENT

With reference to the different angle/axis representations of orientation dis-
placement in Table 2.1, a special case is constituted by the quaternion dis-
placement. Such a representation has the advantage over other angle/axis
representations to avoid representation singularities.
The mutual orientation between d and c can be described by the quater-
 R
nion fdc ; c dc g extracted from c d . Indeed, the orientation displacement to

be considered in (3.62) is given by the vector part c dc and, in view of (3.56),
the expression of the potential energy becomes

Uo = 2cTdcK o cdc, (3.71)

where it has been set = 2. Even though the potential energy is expressed
in terms of the vector part of the quaternion, it can be shown that Uo coincides
with the rotational elastic energy associated with a torsional spring of stiffness
K o acting so as to align c with d .
In view of (3.62), the resulting impedance equation for the rotational part
becomes
M !
c D ! c K 
o
_ dc + o
dc + o dc = ,
0c c  (3.72)
where the rotational stiffness matrix is

K 0o = 2E T(dc ; cdc)K o (3.73)

E
with as in (A.38).
In the case of free motion, it is worth finding the equilibria of the rotational
impedance equation (3.72). These should occur whenever d and c are
aligned.
Consider the Hamiltonian contribution

Ho = To + Uo; (3.74)

associated with the rotational motion, which is a positive definite function.

Taking the time derivative of (3.74) and accounting for (3.55) and (3.56) along
with (3.72) and (3.73) yields

H_ o = ?
c!TdcDo
c!dc + cT
c!dc. (3.75)

 Indirect Force Control 51

 !
If c = 0, H_ o in (3.75) vanishes if and only if
c dc = 0; hence from (3.72)

it follows that c dc asymptotically tends to the invariant set described by
c = 2 (dc K o c dc + S (c dc )K o c dc ) = 0
E (3.76)

where (3.73) has been exploited.

By observing that the two terms in (3.76) are mutually orthogonal, the
following sets of equilibria are found:

E1 = fdc = 1; cdc = 0;
c!dc = 0g (3.77)

E2 = fdc = 0; dc : K o dc = oi dc; k dck = 1;
!dc = 0g(3.78)
c c c c c

where oi > 0 denotes an eigenvalue of matrix o . K

The equilibria in E2 are unstable. To see this, consider the Hamiltonian
contribution (3.74) which, in view of (3.75), is a decreasing function. At any
of the equilibria in (3.78), it is

Ho;1 = 2 oicTdccdc = 2 oi (3.79)

where (A.31) has been used. Consider a small perturbation around the equi-

librium with dc =  , c dc such that c T c  
dc dc = 1 ?  ,
dc = 0 and
2 c !
K  c c 
o dc = oi dc . The perturbed Hamiltonian contribution is
Ho; = 2 oi (1 ? 2 ) < Ho;1 (3.80)

and thus, since (3.74) is decreasing, Ho will never return to Ho;1 , implying
that those equilibria are all unstable. Notice that, at such equilibria, d is anti-
R
aligned with c with respect to the axis of the mutual rotation c d between the
two frames.

It can be concluded that c dc must converge to E1 . Interestingly enough,
R
the two equilibria in E1 both give the same mutual orientation c d = , thus I
implying the alignment of d with c , so as wished.
For the implementation of the inner motion loop in Fig. 3.4, with reference
to the quaternion error used in (2.72), the angular acceleration is taken as

ao = !_ c + K Do
!ce + K PoReece. (3.81)

Notice that Rc , ! c and !_ c can be computed by forward integration of the

rotational impedance equation (3.72).

3.4 EXPERIMENTS
The above impedance control schemes have been tested in a number of
experiments on the six-joint and the seven-joint industrial robots with open
control architecture described in Section 3. of Chapter 1 and force/torque
52 ROBOT FORCE CONTROL

sensor. The kinematic model and dynamic model of the seven-joint robot
manipulator are given in Appendix B
An end effector has been built as a steel stick with a wooden disk of 5:5 cm
radius at the tip. The end-effector frame has its origin at the center of the disk
and its approach axis normal to the disk surface and pointing outwards.
The performance of the quaternion-based six-DOF impedance control has
been compared with that of the two six-DOF impedance control schemes based
on Euler angles. An analysis of the computational burden for the three control
schemes based on (3.17), (2.26), (3.26) and (3.25) has been carried out for
the available hardware, leading to a total time of: 0:264 ms for the impedance
control using (3.45) and (3.32), 0:230 ms for the impedance control using (3.47)
and (3.38), and 0:195 ms for the impedance control using (3.81) and (3.72).
Details on the computational load in terms of floating-point operations and
transcendental functions number are given in Table 3.1.

Table 3.1. Computational load for the three six-DOF impedance control schemes.

Orientation error Flops Funcs

Classical Euler angles 1420 30
Alternative Euler angles 1467 24
Quaternion 1429 15

First case study: Interaction with environment. The first case study has
been developed to analyze interaction with environment. This is constituted
by a flat plexiglas surface. The translational stiffness at the contact between
the end effector and the surface is of the order of 104 N/m, while the rotational
stiffness for small angles is of the order of 20 Nm/rad.
The task consists of taking the disk in contact with the surface at an angle of
unknown magnitude (Fig. 3.7). The end-effector desired position is required to
make a straight-line motion with a vertical displacement of ?0:24 m along the
Zb -axis of the base frame. The trajectory along the path is generated according
to a fifth-order interpolating polynomial with null initial and final velocities
and accelerations, and a duration of 7 s. The end-effector desired orientation is
required to remain constant during the task. The surface is placed (horizontally)
in the Xb Yb -plane in such a way as to obstruct the desired end-effector motion,
both for the translational part and for the rotational part.
The parameters of the translational part of the six-DOF impedance equa-
tion (3.25) have been set to M I D I K
p = 9 , p = 2000 , p = 700 , while I
the parameters of the rotational part of the six-DOF impedance equation (3.72)
have been set to M ID IK I
o = 0:4 , o = 5 , o = 2 . Notice that the stiffness
matrices have been chosen so as to ensure a compliant behavior at the end
 e e

p_ ; !
e e

p ;R
d d

p_ ; !
d d

p ; !_
d d

p ;R
c c

p_ ; !
c c Indirect Force Control 53

p ; !_
c c

a

f; 
q
q_
p ;R
e e

p_ ; !
e e

Figure 3.7. End effector in contact with plexiglas surface.

effector (limited values of contact force and moment) during the constrained
motion, while the damping matrices have been chosen so as to guarantee a
well-damped behavior.
The gains of the inner motion control loop actions in (3.26) and (3.81) have
K IK IK
been set to Pp = 2025 , Po = 4500 , Dp = Do = 65 . K I
The results in Fig. 3.8 show the effectiveness of the quaternion-based six-
DOF impedance control. After the contact, the component of the position
p p p
error between d and e
de = d ? e along the Zb -axis significantly
deviates from zero, as expected, while small errors can be seen also for the
components along the Xb - and the Yb -axis due to contact friction. As for the
orientation error, all the components of the orientation displacement between d

and e e de significantly deviate from zero since the end-effector frame has to
rotate with respect to the base frame after the contact in order to comply with
the surface. Also, in view of the imposed task, a prevailing component of the
contact force can be observed along the Zb -axis after the contact, while the
small components along the Xb - and the Yb -axis arise as a consequence of the
above end-effector deviation. As for the contact moment referred to d , the
component about the Zb -axis is small, as expected. It can be recognized that all
the above quantities reach constant steady-state values after the desired motion
is stopped. The oscillations on the force and moment during the transient can
be mainly ascribed to slipping of the disk on the surface after the contact.
In sum, it can be asserted that a compliant behavior is successfully achieved.
A similar performance has been obtained also with the six-DOF impedance
control schemes based on the Euler angles error, i.e. by using either (3.32)
or (3.38) in lieu of (3.72). This fact can be explained because both the absolute
 b O
bX
b Y
b Z
q1
q2
q3
q4
e p
e n
e s
e a 54 ROBOT FORCE CONTROL
p ;R
d d

p_ ; !
d d

p ; !_
d d position error orientation error
a 0.03 0.4
n
 z
q 0.02 0.2
q_
p ;R e z
[m]

p_ ; !
e e
0.01 0
p ;R
d d
y
p_ ; !
d
x
d

p ; !_ 0 −0.2 x
d d
y
a
 −0.01 −0.4
f;  0 5 10 0 5 10
q [s] [s]
q_
p ;R
e e
contact force contact moment
p_ ; !
e e
0.5
p ;R
d d
40 z
p_ ; !
d d 0
p ; !_
d d
30 x
p ;R
c c
−0.5
p_ ; !
[Nm]

20
[N]

c c

p ; !_ z y
c c
−1
a 10
 x
f;  0 y
−1.5
q
q_ −10 −2
p ;R
e e 0 5 10 0 5 10
p_ ; !
e e
[s] [s]

Figure 3.8. Experimental results under six-DOF impedance control based on quaternion in the
first case study.

end-effector orientation in (3.32) and the relative orientation in (3.38) keep far
from representation singularities. The results are not reported here for brevity.

Second case study: Representation singularity. The second case study is

aimed at testing the performance of the quaternion-based six-DOF impedance
control when the end-effector orientation is close to a representation singularity
T
of . The end effector and the surface are the same as in the previous case
study.
The end-effector desired position is required to make a straight-line motion
with a horizontal displacement of 0:085 m along the Xb -axis of the base
frame. The trajectory along the path is generated according to a fifth-order
interpolating polynomial with null initial and final velocities and accelerations,
and a duration of 5 s. The end-effector desired orientation is required to remain
constant during the task. The surface is now placed vertically in such a way as
to obstruct the desired end-effector motion, only for the rotational part though.
Therefore, no impedance control has been accomplished for the translational
p
part, i.e. c in (3.26) coincides with d . p
p ;R
d d

p_ ; !
d d

p ; !_
d d

a
n 

q
q_
p ;Re

p_ ; !O
e
b
e

p ; RX
e
b
d

p_ ; !Y
d
b
d

p ; !Z_
d
b
d
qa1
d

q2
f ;q3 Indirect Force Control 55
q q4
e
q_ p
p ; Rn
e
e
contact force contact moment
p_ ; !s
e
e
e 2
p ; Ra
e
e

pp_ ;; R!
d d

d d 20
y 1
pp_ ;; !!_
d d
d d y
pp ;; R !_
d d
d
d
c c 0 z 0
p_ ; !a x

[Nm]
[N]

c

c

p ;!

n
_
x z
c c
−20
a −1
q
f ;q_ −40 −2
p ;qR
e e

e p_ q;_ !
e

pp ;; R −60 −3
d d
R 0 5 10 0 5 10
_p ; !
e e

d
p_ ; !
d
[s] [s]
p ; !_
e e

d d

a Figure 3.9. Experimental results under six-DOF impedance control based on quaternion in the

f ; second case study.
q
q_
p ;R
e e
contact force contact moment
e p_ ; !
e 2
p ;R y
d d

p_ ; ! 20 y
d d 1
dp ; !_
d
z
cp ;R
c 0 0 x
p_ ; !
[Nm]
[N]

c c

c p ; !_
c
−20
a −1
 x z
f ;  −40 −2
q
q_ −60 −3
p ;R
e e 0 5 10 0 5 10
e p_ ; !
e [s] [s]

Figure 3.10. Experimental results under six-DOF impedance control based on the classical
Euler angles in the second case study.

The parameters of the quaternion-based impedance equations (3.25) and

(3.72) are set to M I D I K
p = 10 , p = 600 , p = 1000 , o = 0:25 , I M I
D IK I
o = 3:5 , o = 2:5 . In order to carry out a comparison, the impedance
control based on the Euler angles has also been tested. The parameters of the
rotational impedance equation (3.32) have been set to the same values as for
the quaternion. As regards the gains of the inner motion control loop, these
have been chosen equal to those in the previous experiment for both types of
impedance control schemes.
The results in Figs. 3.9 and 3.10 show the significant differences occurring in
the performance of the two schemes. For the impedance control based on (3.32),
large values of contact force and moment are generated since the rotational
impedance equation suffers from ill-conditioning of the matrix ( c ); this T'
phenomenon is not present for the quaternion-based impedance control based
p_ ; !
d d

p ; !_
d d

a
n

q
q_
p ;R
e e

p_ ; !
e e

p ;R
d d

p_ ; !
d d

p ; !_
d d

a

f ; 56 ROBOT FORCE CONTROL
q
q_
p ;R
e e
orientation misalignment applied moment
p_ ; ! 0.3
e e

p ;R
d d
2.5
z
p_ ; !
d d

p ; !_ 0.2
d d 2
p ;R
c c

p_ ; ! 1.5

[Nm]
c c

p ; !_ 0.1
c c
1
a
 0.5
f;  x, y
q 0 0
q_ −0.5
p ;R
e e 0 2 4 6 8 10 0 2 4 6 8 10
p_ ; !
e e [s] [s]

Figure 3.11. Experimental results under six-DOF impedance control based on quaternion in
the third case study.

on (3.72) since representation singularities are not involved in the rotational

impedance equation. On the other hand, testing of the impedance control based
on the alternative Euler angles in (3.38) has revealed a performance as good as
the quaternion-based impedance control, since the orientation displacement dc '
is kept far from a representation singularity. Hence, the results are not reported
here for brevity.
In sum it can be concluded that both the impedance control based on the
alternative Euler angles and the quaternion-based impedance control perform
better than the impedance control based on the classical Euler angles, as far as
interaction with the environment is concerned.

Third case study: Task geometric consistency. Another case study has been
developed to analyze task geometric consistency when an external moment is
applied at the end effector. The quaternion-based impedance control and the
impedance control based on Euler angles have been tested.
The stiffness matrices of the rotational part of the impedance equations (3.72)
and (3.32) have been taken as diagonal matrices; K
o has been chosen as
U I I
in (3.66) with o = and ?o = 2:5 for both schemes. The remaining
parameters of the rotational impedance have been set to M
o = 0:25 and I
D I
o = 1:5 for both schemes. No impedance control has been accomplished
for the translational part. The gains of the inner motion control loop have been
chosen equal to those in the previous case study.
The position and orientation of the desired frame are required to remain
constant, and a torque is applied about the approach axis of d ; the torque is
taken from zero to 2:5 Nm according to a linear interpolating polynomial with
4th-order blends and a total duration of 1 s.
p_ ; !
d d

p ; !_
d d

a
n

q
q_
p ;R
e e

p_ ; !
e e

p ;R
d d

p_ ; !
d d

p ; !_
d d

a

f;  Indirect Force Control 57
q
q_
p ;R
e e
orientation misalignment applied moment
p_ ; ! 0.3
e e

p ;R
d d
2.5
z
p_ ; !
d d

p ; !_ 0.2
d d 2
p ;R
c c

p_ ; ! 1.5

[Nm]
c c

p ; !_ 0.1
c c
1
a
 0.5
f;  x, y
q 0 0
q_ −0.5
p ;R
e e 0 2 4 6 8 10 0 2 4 6 8 10
p_ ; !
e e [s] [s]

Figure 3.12. Experimental results under six-DOF impedance control based on classical Euler
angles in the third case study.

The results in Figs. 3.11 and 3.12 show the different performance in terms
of the orientation misalignment  which has been defined as the norm of the
vector product between the orientation error and the unit vector of the approach
u
axis of d o3 , i.e.
 = kS (e de )uo3 k:
For the impedance control based on (3.32) the instantaneous axis of rotation
of e changes, while remarkably no misalignment occurs for the impedance
control based on (3.72). The impedance control based on (3.38) has also been
tested and its performance is as good as that of the quaternion-based control;
hence, the results are not reported for brevity.

Fourth case study: Nondiagonal rotational stiffness. In the fourth case

study, the quaternion-based impedance control and the impedance control based
on the alternative Euler angles have been tested when the rotational stiffness is
chosen as a nondiagonal matrix. The impedance control based on the classical
Euler angles has been ruled out in view of the poor results of the previous
experiment.
The principal axes of the stiffness matrices of the rotational impedance
equations (3.72) and (3.38) are rotated with respect to the coordinate axes
K
of d ; o has been chosen as in (3.66) with
2
0:8047 ?0:3106 0:5059 3 2
4 0 0 3
U o = 4 0:5059 0:8047 ?0:3106 5 ?o = 4 0 1 0 5
?0:3106 0:5059 0:8047 0 0 2:5
for both schemes. The remaining parameters of the rotational impedance have
been set to M I D I
o = 0:25 and o = 1:5 for both schemes. As above, no
impedance control has been accomplished for the translational part, and the
p ;R
d d

p_ ; !
d d

p ; !_
d d

a
n 

q
q_
p ; RO
e e
b

e p_ ; X
e
b
!
p ; RY
d d
b

dp_ ; !Z
d
b
p ; !_q1
d d

qa2
q3
f ;q458 ROBOT FORCE CONTROL
e
qp
e
q_ n
p ; Rs
e e
e
orientation misalignment applied moment
e p_ ; !a 0.15
e
e
2
pp ;; R
d
d
d
d
R
pp__ ;; !!
d
d
d
d
1
pp ;; !!__ 0.1
d
d
d
d y
cp ; Ra
c

p_ ;!

[Nm]
c c
n 0
c p ; !_
c

qa 0.05 x
q_ −1
p f; R; 
e e
z
e p_ q; !
e 0
p ;q_R
d d −2
p_ ;; R
e
d
e
d ! 0 2 4 6 8 10 0 2 4 6 8 10
e
d p_ ;; !
e
d _ [s] [s]
a
 Figure 3.13. Experimental results under six-DOF impedance control based on quaternion in
f ; the fourth case study.
q
q_
p ;R
e e
orientation misalignment applied moment
e p_ ; ! 0.15
e
2
p ;R
d d

dp_ ; !
d

p ; !_ 1
d d
y
cp ; R 0.1
c

p_ ; !
[Nm]

c c
0
c p ; !_
c

a 0.05 x
 −1
f;  z
q 0
q_ −2
p ;R
e e 0 2 4 6 8 10 0 2 4 6 8 10
e p_ ; !
e [s] [s]

Figure 3.14. Experimental results under six-DOF impedance control based on alternative Euler
angles in the fourth case study.

gains of the inner motion control loop have been chosen equal to those in the
previous case study. A torque has been applied about the axis whose unit
u
vector is o3 ; the torque is taken from zero to ?1:5 Nm according to a linear
interpolating polynomial with 4th-order blends and a total duration of 1 s.
The results in Figs. 3.13 and 3.14 show the significant differences occurring
in terms of the orientation misalignment  . It can be seen that the instantaneous
axis of rotation of e does not appreciably rotate with the impedance control
based on (3.72), given the performance of the inner loop acting on the end-
effector orientation error. Instead, a significant misalignment occurs with the
impedance control based on (3.38).
In sum, it can be concluded that the quaternion-based impedance control
performs better than both impedance control schemes based on the Euler angles
as far as task geometric consistency is concerned.
 
f; 
q
q_
p ;R
e e

p_ ; !
e e

p ;R
d d

p_ ; !
d d

p ; !_
d d

p ;R
c c

p_ ; !
c c
Indirect Force Control 59
p ; !_
c c

a
 Y b

f; 
q Z e

q_
p ;R
p_ ; ! Z
e e
b

Y
e e
e

Figure 3.15. Disk in contact with surface.

Fifth case study: Redundancy resolution. The six-DOF quaternion-based

impedance control has been tested in a case study when a redundancy resolution
scheme is incorporated into the motion control as in Fig. 2.2.
The environment is constituted by a cardboard box. The translational stiff-
ness at the contact between the end effector and the surface is of the order
of 5000 N/m, while the rotational stiffness for small angles is of the order of
15 Nm/rad.
The task in the experiment consists of four phases; namely, reconfiguring
the manipulator, approaching the surface, staying in contact, and leaving the
surface. To begin, the additional task function in (2.91) has been chosen as

w(q) = 12 (q3 ? q3d)2

where q3 is the elbow joint and q3d is a desired trajectory from the initial value
of q3 to the final value of 1:1 rad in a time of 4 s with a fifth-order interpolating
polynomial with null initial and final velocity and acceleration. This function
is aimed at reconfiguring the manipulator in a more dexterous posture before
contacting the surface.
After a lapse of 4 s, the disk is taken in contact with the surface at an angle
 = 7=36 rad; see Fig. 3.15 where the orientation of the base and end-effector
frames is depicted. The end-effector desired position is required to make a
straight-line motion with a horizontal displacement of 0:08 m along the Zb
axis of the base frame. The trajectory along the path is generated according to
a fifth-order interpolating polynomial with null initial and final velocities and
accelerations, and a duration of 2 s. The end-effector desired orientation is
required to remain constant during the task. The surface is placed (vertically)
in the Xb Yb -plane of the base frame in such a way to obstruct the desired end-
effector motion, both for the translational part and the rotational part. After
 bX
b Y
b Z
q1
q2
q3
q4
e p
e n
e s
e a
p ;R
d d

p_ ; !
d d

p ; !_
d d

a
n 60 ROBOT FORCE CONTROL

q
q_ position error orientation error
p ;R
e e 0.03
p_ ; !
e e

p ;R
d d z
p_ ; !
d d
0.02 xz
p ; !_
d d
0
a
[m]

0.01

f; 
q x −0.1
0 y
q_
p ;R e
y
e

p_ ; !
e e
−0.01 −0.2
p ;R
d d 0 10 20 30 40 0 10 20 30 40
p_ ; !
d d [s] [s]
p ; !_
d d

p ;R
c c
contact force contact moment
p_ ; !
c c
20 2
p ; !_
c c

a 10 xy 1.5
 0 1 y
f; 
q
[Nm]
[N]

−10 0.5
q_ xz
p ;R
e e
−20 z 0
p_ ; !
e e

 −30 −0.5
b Z
b Y −40 −1
e Z 0 10 20 30 40 0 10 20 30 40
e Y [s] [s]

Figure 3.16. Experimental results under six-DOF impedance control based on quaternion in
the fifth case study.

a lapse of 13 s in contact, the end-effector motion is commanded back to the

initial position with a duration of 4 s.
The parameters of the translational impedance (3.25) have been set to p = M
ID K
16 , p = diagf800; 800; 250g and p = diagf1300; 1300; 800g, while the
parameters of the rotational impedance (3.72) have been set to o = 0:7 and M I
D IK
o = 4 , o = 2:5 . I
The gains of the inner motion control loop in (3.26) and (3.81) have been
K I K IK
set to Pp = 2250 and Po = 4000 , Dp = 70 and Do = 75 . The I K I
gains of the redundancy resolution control in (2.89) and (2.91) have been set to
K I
n = 20 and k = 250.
The results in Fig. 3.16 show the effectiveness of the six-DOF impedance
control with redundancy resolution. During the reconfiguration (8 s), the
p
components of the position error between d and e
de = d ? e and of p p

the orientation error between d and e e de are practically zero, meaning that
the dynamics of the null space motion does not disturb the end-effector motion.
Such error remains small during the approach (2 s). During the contact (13 s),
the component of the position error along the Zb -axis significantly deviates
 q
q_
p ;R
e e

p_ ; !
e e

p ;R
d d

p_ ; !
d d

p ; !_
d d

a

f; 
q
q_
p ;R
e e

p_ ; !
e e

p ;R
d d Indirect Force Control 61
p_ ; !
d d

p ; !_
d d

p ;R
c c
additional task function
p_ ; !
c c
1.5
p ; !_
c c

a
 1
f; 

[rad^2]
q
q_
p ;R
e e
0.5
p_ ; !
e e


Z b

Y b 0
Z e
0 10 20 30
Y e
[s]

Figure 3.17. Additional task function in the fifth case study.

from zero, as expected; as for the orientation error, the component of the
orientation error along the Ye -axis significantly deviates from zero since e
has to rotate about Ye in order to comply with the surface. Also, in view of
the imposed task, a prevailing component of the contact force can be observed
along the Zb -axis after the contact, whereas the sole component of the contact
moment about the Ye -axis is significant, as expected. During the takeoff (4 s),
both the errors and the contact force and moment return to zero.
The same task has been executed again for the six-DOF impedance control
without redundancy resolution (k = 0). The performance in terms of the
contact between the end effector and the surface is substantially the same as
above since the additional task does not interfere with the primary interaction
task; hence, the time history of the relevant quantities is omitted for brevity.
Nevertheless, a comparison between the two cases in Fig. 3.17 shows that the
task function is successfully optimized when redundancy is exploited (solid)
other than when redundancy is not exploited (dashed).

3.5 NONDIAGONAL SIX-DOF STIFFNESS

In the above treatment of the six-DOF impedance, it has been implicitly
assumed that the translational stiffness is separated from the rotational stiffness.
Nevertheless, the general form of a six-DOF stiffness may contain off-diagonal
terms, which may be useful to accomplish certain compliant tasks, e.g. to
prevent jamming during parts mating.
The quaternion displacement is considered hereafter in view of its task
geometric consistency property and avoidance of representation singularities.
A coupling elastic energy can be added to Up in (3.29) and Uo in (3.71), leading
62 ROBOT FORCE CONTROL

to

U = 21
cpTdcK p
cpdc + 2cTdcK ocdc + 2dc cTdcK m
cpdc (3.82)

where K m is a constant matrix such that the nondiagonal stiffness matrix


K
K = K m Kmop K T 
(3.83)

is positive definite. In (3.82) the position displacement has been referred to c

for consistency with the orientation displacement. A sufficient condition for
the positive definiteness of K is

m (K o ) m (K p) > M2 (K m ) (3.84)

where m (
) ( M (
)) denotes the minimum (maximum) eigenvalue and M (
)
denotes the maximum singular value of a matrix. Notice that, being jdc j 
p 
1, the function U is positive definite in the variables
c dc and c dc under
condition (3.84).
Taking the time derivative of (3.82) yields

U_ = cf TE
cp_ dc + cTE
c!dc (3.85)

where
cf
E = K p
pdc + 2K m dc
c Tc
(3.86)
c = GT ( ; c  )K m
cp + 2E T ( ; c  )K o c 
E dc dc dc dc dc dc (3.87)

with
G = dcE ? cdccTdc, (3.88)
are respectively the elastic force and moment which contain coupling terms, i.e.
the orientation displacement appears in (3.86) while the position displacement
appears in (3.87).
Therefore, the equations of the translational and the rotational impedance
(3.25) and (3.72) become

M p
cp dc + Dp
cp_ dc + K p
cpdc + 2K Tm cdc = cf (3.89)

M o
c!_ dc + Do
c!dc + K 0m
cpdc + K 0o cdc = c (3.90)

where the terms in (3.89) have all been referred to c , and

K 0o = 2E T(dc; cdc)K o (3.91)

K 0m = GT(dc ; cdc)K m . (3.92)
 Indirect Force Control 63

The stiffness analysis for small position and orientation displacements be-
tween d and c shows that
c f ' K
c p_ dt + K T
c! dt
E p dc m dc (3.93)
c ' K
cp_ dt + K
c ! dt
E m dc o dc (3.94)

and thus K is the stiffness matrix for small displacements.

As regards the property of task geometric consistency, taking the coupling
K K
stiffness as a symmetric matrix m = T m allows for the decomposition
K m = U m ?mU Tm (3.95)

where ?m = diagf m1 ; m2 ; m3 g and U m = [ um1 um2 um3 ] are re-

spectively the eigenvalue matrix and the (orthogonal) eigenvector matrix. Then,
considering a null position displacement and an orientation displacement by an
angle #dc about the i-th eigenvector gives

cf
E = 2 mi sin #2dc umi (3.96)

u
which represents an elastic force along the same mi axis. On the other hand,
considering a null orientation displacement and a position displacement of
length  along the i-th eigenvector gives
c =  mi umi
E (3.97)

u
which represents an elastic moment about the same mi axis. Therefore, the
K
coupling stiffness matrix m can be expressed in terms of three parameters mi
representing the coupling stiffness about three principal axes mi . u
Finally, the computation of the equilibria of the impedance equations (3.89)
and (3.90) with the relative stability analysis can be carried out in a conceptually
analogous way to Subsection 3.3.

4. FURTHER READING
Stiffness control was proposed in [87] and is conceptually equivalent to
compliance control [82]. Devices based on the remote center of compliance
were discussed in [110] for successful mating of rigid parts. The original idea
of a mechanical impedance model used for controlling the interaction between
manipulator and environment is due to [50], and a similar formulation is given
in [55].
The adoption of an inner motion control loop to provide impedance control
with disturbance rejection was presented to [64], and has been experimentally
demonstrated for an industrial robot with joint friction in [14]. Linear [31] and
nonlinear [56, 22] adaptive impedance control algorithms have been proposed
64 ROBOT FORCE CONTROL

to overcome model uncertainties, while robust schemes can be found in [65].

Impedance control has also been used in the hybrid position/force control
framework [4]. Emphasis on the problems with a stiff environment is given
in [54] and stability of impedance control was analyzed in [51]. The automatic
specification of robotic tasks that involve multiple and time-varying contacts
of the manipulator end-effector with the environment is a challenging issue.
General formalisms addressing this problem were proposed in [83, 15].
A reference work on modeling six-DOF (spatial) stiffness is [61] while
the properties of spatial compliance have been analyzed in detail in [80, 41];
a six-DOF variable compliant wrist was proposed in [52]. The energy-based
approach to derive a spatial impedance was introduced in [42] and later adopted
in [95] for active impedance control. The various six-DOF impedance control
schemes with different representations of end-effector orientation can be found
in [19] with full experimental support in [17]. The technique for redundancy
resolution is based on [73, 74]. In [20, 21] the quaternion-based six-DOF
impedance control is extended to the case of nondiagonal stiffness which can
be useful for the execution of specific compliant tasks as discussed in [5].
Chapter 4

DIRECT FORCE CONTROL

Direct force control schemes are developed which achieve force regulation when the end
effector is in contact with a compliant environment, thanks to the adoption of an integral
action on the force error generated by an outer force loop. Motion control capabilities
along the unconstrained task directions are recovered using a parallel composition of the
force and motion control actions. Force tracking along the constrained task direction
is achieved by adapting the estimate of the contact stiffness. Throughout the chapter,
experimental results are presented for an industrial robot in contact with a compliant
surface.

1. FORCE REGULATION
In the previous chapter, an indirect control of the contact force has been
achieved by suitably controlling the end-effector motion. In this way, it is
possible to ensure limited values of the contact force for a given rough estimate
of the environment stiffness. Certain interaction tasks, however, do require the
fulfillment of a precise value of the contact force. This would be possible, in
theory, by tuning the active compliance control action and selecting a proper
desired location for the end effector; such a strategy would be effective only on
the assumption that an accurate estimate of the contact stiffness is available.
A radically different approach consists in designing a direct force control
which operates on a force error between the desired and the measured values.
On the other hand, in the previous chapter it has been emphasized that even
impedance control, which does not aim at achieving a desired force, needs
contact force measurements to obtain a linear and decoupled end-effector dy-
namics during the interaction. This is desirable in order to realize a compliant
behavior only along those task directions that are actually constrained by the
presence of the environment. Therefore, contact force measurements are fully
exploited hereafter to design direct force control.
65
66 ROBOT FORCE CONTROL

The realization of a force control scheme can be entrusted to the closure

of an outer force control loop generating the reference input to the motion
control scheme the robot manipulator is usually endowed with. Therefore,
direct force control schemes are presented below which are logically derived
from the motion control schemes using a static model-based compensation and
a dynamic model-based compensation, respectively. In this respect, it should
be stressed that the force control problem basically requires regulation of the
contact force to a constant desired value.

1.1 STATIC MODEL-BASED COMPENSATION

Let f d and d denote the constant desired contact force and moment. With
reference to the static model-based compensation in (2.92), the vectors p
and o can be chosen as
p = K Pp
pce + f d (4.1)
o = T ('e)K Po
'ce + d
? T
(4.2)
with
pce in (3.27) and
'ce in (3.46). Notice that pc and 'c represent the
position and orientation of a compliant frame c which shall be determined
through a proper force and moment control action. The terms f d in (4.1)
and d in (4.2) represent a force and moment feedforward aimed at creating
the presence of a force and moment error as

f = f d ? f (4.3)

 =  d ?  (4.4)
p '
in the closed-loop equation of the system. Then, the vectors c and c can be
chosen as a proportional–integral (PI) control on the force and moment error,
i.e.
 Z t 
pc = K ?Pp1 K Fp
f + K Ip 0
f d& (4.5)
 Z t 
'c Po K Fo
 + K Io 0
d&
= K ?1 (4.6)

where K Fp , K Ip , K Fo and K Io are suitable positive definite matrix gains.

Notice that Equations (4.5) and (4.6) set an outer control loop on the contact
force and moment, with respect to the inner control loop on the end-effector
K
position and orientation in (4.1) and (4.2). To this regard, ? 1
K ?1
Pp in (4.5) and Po
in (4.6) have been introduced in order to make the resulting force and moment
control actions in (4.1) and (4.2) independent of the choice of the proportional
gain on the position and orientation, respectively.
On the assumption that the desired contact force and moment are assigned so
that they have nonnull components only along the constrained task directions,
p ; !_
c c

a

f; 
q
q_
p ;R
e e

p_ ; !
e e


Zb

Yb
Direct Force Control 67
Ze

Ye f; 
f ;
d d FORCE & MOM p ;R
c c POS & ORIENT a INVERSE  MANIPULATOR q
CONTROL CONTROL DYNAMICS & ENVIRONMENT q_

p ;Re e

p_ ; !
e e
DIRECT
KINEMATICS

Figure 4.1. Force and moment control with inner motion control loop.

the control scheme based on (4.1), (4.2), (4.5) and (4.6) gives at steady state

f1 = fd (4.7)
1 = d (4.8)

thanks to the use of integral control actions on the force and moment errors.
On the other hand, the proportional actions on the force and moment errors are
aimed at improving the transient behavior during the interaction.
In sum, regulation of the contact force and moment to the desired values can
be obtained, provided that the control gains are properly chosen so as to ensure
stability of the closed-loop system.

1.2 DYNAMIC MODEL-BASED COMPENSATION

In order to enhance the performance of the system during the transient,
it is worth considering dynamic model-based compensation as in (3.17) with
 as in (2.26). With reference to (3.26) and (3.45), the linear and angular
accelerations can be respectively chosen as

ap = ?K Dpp_ e + K Pp
pce (4.9)

ao = T ('e) (?K Do'_ e + K Po
'ce) + T_ ('e; '_ e)'_ e (4.10)
where pc and 'c are given in (4.5) and (4.6). Notice that, differently from (3.26)
and (3.45), no feedforward of the linear and angular velocity and acceleration
of c has been used since the goal is simply to achieve force regulation. As
above, at steady state, the contact force and moment are taken to the desired
values assuming that these are assigned consistently with the constrained task
directions.
A block diagram of the resulting force and moment control scheme with
dynamic model-based compensation is sketched in Fig. 4.1 where the presence
of the inner motion control loop has been evidenced. Also, the orientation
 
f; 
q
q_
p ;R
e e

p_ ; !
e e

p ;R
d d

p_ ; !
d d

p ; !_
d d

p ;R
c c

p_ ; !
c c

p ; !_
c c

a

f; 
q 68 ROBOT FORCE CONTROL
q_
p ;R
e e

p_ ; !
e e
end−effector path contact force
 40
b Z 0
b Y 30
e Z
e Y −0.05 z
20
f ;
z [m]

[N]
d d

p ;R
c c

a −0.1 10
 x, y
f;  0
q −0.15
q_ −10
p ;R
e e −0.1 −0.05 0 0.05 0.1 0 5 10 15 20
p_ ; !
e e y [m] time [s]

Figure 4.2. Experimental results under force control with static model-based compensation.

of the relevant frames has been described in terms of rotation matrices. With
respect to the impedance control scheme with inner motion control in Fig. 3.4,
the impedance control is replaced with a force and moment control which
generates the position and orientation of c according to (4.5) and (4.6).

1.3 EXPERIMENTS
The above force and moment control schemes have been tested in exper-
iments on the six-joint industrial robot described in Section 3. of Chapter 1
endowed with the force/torque sensor.
Only the inner three joints are used while the outer three joints are mechan-
J J
ically braked. Hence n = 3 in (2.92) with = p and = p ; accordingly, a
three-DOF task is considered.
The environment and the end effector are those described in Subsection 1.2 of
Chapter 3. The end effector is placed in the same initial position as for the case
study in that subsection but, of course, no position trajectory is assigned. The
desired force along Zb is taken to 20 N according to a trapezoidal velocity profile
with cubic blends, and null initial and final first and second time derivatives,
and a duration of 2 s. The constant value is kept for the remaining portion of
the task.

First case study: Force control with static model-based compensation.

The gains of the various control actions have been set to Pp = 300000 K I
K I K
in (4.1), D = 3000 in (2.92), while Fp = and Ip = 150 in (4.5); O K I
note that the proportional action on the force error does not vanish because of
K
the null Fp since the force feedforward in (4.1) combined with the contact
force term in (2.20) is in turn equivalent to a unitary proportional action.
The results are presented in Fig. 4.2 in terms of the end-effector path, together
with the time history of the desired (dashed) and the actual (solid) contact
 
f; 
q
q_
p ;R
e e

p_ ; !
e e

p ;R
d d

p_ ; !
d d

p ; !_
d d

p ;R
c c

p_ ; !
c c

p ; !_
c c

a

f; 
q Direct Force Control 69
q_
p ;R
e e

p_ ; !
e e
end−effector path contact force
 40
Z
b
0
Y
b 30
Z
e

Y
e −0.05 z
20
f ;
z [m]

[N]
d d

p ;R
c c

a −0.1 10
 x, y
f;  0
q −0.15
q_ −10
p ;R
e e −0.1 −0.05 0 0.05 0.1 0 5 10 15 20
p_ ; !
e e y [m] time [s]

Figure 4.3. Experimental results under force control with dynamic model-based compensation.

force. As usual, the approximate location (dotted) of the surface is illustrated

on the plot of the end-effector path, while the instant of contact (dotted line) is
evidenced on the plot of the contact force.
Initially, the desired force trajectory causes a downward vertical motion
since the end effector is required to push in the air; this brings the end effector
to come in contact with the surface at t = 12 s. Then, the contact force is
successfully regulated to the desired value. The components of contact friction
force along Xb and Yb are nearly zero since no motion is commanded along
those directions.

Second case study: Force control with dynamic model-based compensa-

tion. The gains of the various control actions have been set to Dp = 90 K I
K I K
and Pp = 2500 in (4.9), while Fp = 0:125 and Ip = 37:5 in (4.5). I K I
The results are presented in Fig. 4.3 in terms of the end-effector path, together
with the time history of the desired (dashed) and the actual (solid) contact
force. As above, the approximate location (dotted) of the surface is illustrated
on the plot of the end-effector path, while the instant of contact (dotted line) is
evidenced on the plot of the contact force.
Initially, the desired force trajectory causes a downward vertical motion that
brings the end effector to come in contact with the surface at t = 4:5 s. On the
other hand, the response of the contact force is faster than that in Fig. 4.2 with
pure force control thanks to the use of a dynamic model-based compensation
which allows obtaining a larger bandwidth of the force loop without affecting
stability.

2. FORCE AND MOTION CONTROL

With the adoption of a direct force control strategy, regulation of the contact
force has been obtained at the expense of loss of control of the end-effector
70 ROBOT FORCE CONTROL

motion. Nevertheless, it would be desirable to recover motion control along the

unconstrained task directions while ensuring force control along the constrained
task directions. This goal can be achieved only if a detailed description of the
geometry of the environment is available. If that is not the case, an effective
strategy can be pursued where control of both force and motion is carried out
in all task directions; the control actions are to be designed on the basis of a
simplified model of the environment while providing some sort of robustness to
uncertainty. This is the underlying philosophy of the so-called parallel control
approach described hereafter. In view of the difficulties concerned with contact
modeling, the problem of force and position control is treated separately from
that of moment and orientation control.

2.1 FORCE AND POSITION REGULATION

The idea of parallel control is to compose the compliant position in (4.5)
with the desired position as

pr = pc + pd (4.11)

and input this reference position pr , in lieu of pc , to the static model-based

compensation control action in (4.1), i.e.

p = K Pp(pr ? pe) + f d. (4.12)

Then, the control law (2.92) can be rewritten in terms of the quantities involving
force and position only, i.e.

 = J Tp (q) p ? K D q_ + g(q), (4.13)

which, in view of (4.11) and (4.12), shall allow recovering regulation of the
desired end-effector position along the unconstrained task directions if the force
error in (4.5) acts only along the constrained task directions.
In order to gain insight into the behavior during interaction with parallel
control, it is necessary to utilize a model of the environment. To this purpose, a
planar surface is considered, which is locally a good approximation to surfaces
of regular curvature, and the rotation matrix of the compliant frame c

Rc = [ t1c t2c nc ] (4.14)

is conveniently chosen with nc normal and t1c and t2c tangential to the plane.
Thus, the model of the contact force is that given in (3.9), i.e. f = K f (pe ? po )
where po represents the position of any point on the undeformed plane and the
contact stiffness matrix takes on the form

K f = Rcdiagf0; 0; kf;n gRTc = kf;nncnTc (4.15)

 q_
p ;R
e e

p_ ; !
e e


Z b

Y b

Z e

Y e

f ;
d d

p ;R
c c

a Direct Force Control 71


f; 
q k ? 1 n nT f
c
q_ ?
f;n c d

I ? n nT p
p 1
c
p ;R
c d

e e e;
p
p_ ; !
e e
d

p o

O
b n nT p
c c o

Figure 4.4. Equilibrium position with force and position control.

with kf;n > 0. Notice that the orientation of c does not change during the
R
interaction, hence c is a constant matrix; on the other hand, the origin c p
of c changes as a function of the force control action in (4.5).
The elastic contact model (3.9) and (4.15) shows that the contact force is
normal to the plane, and thus a null force error can be obtained only if the
f n
desired force d is aligned with c . Also, it can be recognized that null
position errors can be obtained only on the contact plane ( 1c ; 2c ), while the t t
p n
component of e along c has to accommodate the force requirement specified
f
by d .
q q
The steady state ( _ = 0;  = 0) of the system (2.20) with (4.13) and (4.12)
is
T
J K p p
( Pp( r ? e ) + d ) = T . f
(4.16) J f
On the assumption of a full-rank Jacobian, in view of (4.11) and (4.5), Equa-
tion (4.16) gives
Z 1
K Pp
pde + K Fp
f + K Ip 0

f d& +
f = 0 (4.17)

where the choice of scalar matrix gains is to be made, i.e. Pp = kPp , K I

K I K I
Fp = kFp and Ip = kIp , in order to avoid any rotation of the force and
position vectors.
In view of the contact model (3.9) and (4.15), d is taken along c and thus f n
the equilibrium is described by
   
pe;1 = I ? ncnTc pd + ncnTc kf;n ?1 f + p
d o (4.18)
f 1 = kf;nncnTc (pe;1 ? po) = f d (4.19)

where the matrix (I ? nc nT

c ) projects the vectors on the contact plane.
72 ROBOT FORCE CONTROL

p
In Fig. 4.4 the equilibrium position is depicted when a constant d is assigned.
p p
It can be recognized that e;1 differs from d by a vector aligned along the
normal to the contact plane whose magnitude is that necessary to guarantee
f f
1 = d in view of (4.19).
f n
It might be argued that the assignment of d along c indeed requires the
knowledge of the normal direction to the contact plane. If this conditions does
not hold, then it can be found that a drift motion of the end effector is generated
along the plane. Therefore, if the contact geometry is unknown, it is advisable
f
to set d = 0.
In sum, regulation of the end-effector position to the desired components
along the plane with regulation of the contact force to the desired component
along the normal to the plane can be obtained, provided that the control gains
are properly chosen so as to ensure stability of the closed-loop system. This
issue will be reconsidered in the next chapter, when an adaptive version of this
parallel regulator will be presented.

2.2 FORCE AND POSITION CONTROL

The above control scheme provides regulation of the end-effector position
along the unconstrained task directions. On the other hand, tracking of a
desired time-varying position can be achieved by using a dynamic model-
based compensation. To this purpose, the linear acceleration in (4.9) can be
modified as

ap = p d + K Dp(p_ d ? p_ e) + K Pp(pr ? pe) (4.20)

p
where r is given in (4.11) and feedforward of the desired velocity and accel-
p p
eration has been added in view of the presence of d in r .
The equations of the closed-loop system can be obtained by plugging (4.20)
p
in (2.30) with c as in (4.5), leading to
Z t

pde + K Dp
p_ de + K Pp
pde + K Fp
f + K Ip
f d& = 0. (4.21)
o
f = 0 yields
Projecting (4.21) on the axes of c in (4.14) and taking d;t

pde;t + kDp
p_ de;t + kPp
pde;t = 0 (4.22)

Z t

pde;n + kDp
p_ de;n + kPp
pde;n + kFp
fn + kIp
fnd& = 0 (4.23)
0

K IK IK
where Dp = kDp , Pp = kPp , Fp = kFp and Ip = kIp . I K I
Equation (4.22) describes the dynamic behavior of the components of the
position error on the contact plane. Stability is obtained for any choice
of kDp ; kPp > 0 and tracking of the desired end-effector position on the
contact plane is ensured.
 Direct Force Control 73

On the other hand, Equation (4.23) involves the normal components of both
the position error and the force error. In view of the model (3.9) and (4.15), it
is
?1
fn ? dn

pde;n = kf;n (4.24)
with
?1 fd;n ? pd;n + po;n ,
dn = kf;n (4.25)
which allows rewriting (4.23) as
Z t
? 1  ? 1 _ ?
kf;n
fn + kf;nkDp
fn +(kf;n kPp + kFp)
fn + kIp
fnd& = n (4.26)
1
0

where
n = dn + kDpd_n + kPp dn. (4.27)
Stability of the third-order system described by (4.26) can be ensured by choos-
ing the gains kDp , kPp , kFp and kIp so as to satisfy the condition
?1 kPp + kFp )
kIp < kDp (kf;n (4.28)

which requires a rough estimate of the stiffness coefficient kf;n . Further, if

dn is constant, then n is constant and regulation of the contact force to the
desired component along the normal to the plane is ensured.
Notice that, since po;n and fd;n in (4.25) have been assumed to be constant,
dn is constant as long as pd;n is constant, i.e. the desired trajectory must have
null velocity and acceleration along the normal to the plane.
On the other hand, if pd;n is not constant, then in view of (4.26) the position
trajectory acts as a disturbance on the dynamics of the force error. Such
a disturbance n is bounded as long as the desired position, velocity and
acceleration are bounded. The presence of an integral action on the force error
confers disturbance rejection capability to the system while guaranteeing null
force error at steady state for a constant disturbance. This feature makes the
scheme inherently robust to unplanned contact situations. In fact, if contact
occurs along a faulty supposedly unconstrained task direction (where a null
desired force is assigned), control provides an immediate recovery by keeping
the contact force bounded at the expense of a position error along that direction.
A block diagram of the resulting force and position control scheme with
parallel composition is sketched in Fig. 4.5. Apart from the lack of control
of orientation and moment, this scheme differs from the force control scheme
in Fig. 4.1 in that the position of the compliant frame is added to the desired
position as in (4.11) generating the position of a reference frame r for the
position control; this receives as input also the desired velocity and acceleration.
The selection of the control gains for the above parallel control scheme
shall be accomplished so as to achieve a satisfactory behavior during both
 q_
p ;R e e

p_ ; ! e e

O b

n nT p c c o

?
p o
I ? n nT p c c d

k?1 n nT f
f;n c c d

p 1 e;

p d

74 ROBOT FORCE CONTROL

p d p p_
d d

f
f d FORCE p c PARALLEL p r POSITION a p INVERSE  MANIPULATOR q
CONTROL COMPOSITION CONTROL DYNAMICS & ENVIRONMENT q_

p
p_
e
DIRECT
e KINEMATICS

Figure 4.5. Force and position control with parallel composition.

unconstrained and constrained motion. It might be argued, however, that the

choice of the gains for the force control action does depend on the choice of
the gains for the position control action.
A modified parallel control scheme can be devised with the goal of design-
ing the force control action independently of the position control action. In
particular, the parallel composition in (4.11) can be extended to velocity and
acceleration, i.e.
p_ r = p_ c + p_ d (4.29)
p r = p c + p d (4.30)
and thus the linear acceleration in (4.20) is modified into
ap = p r + K Dp(p_ r ? p_ e) + K Pp(pr ? pe) (4.31)
p
where c in (4.11) is the solution to the differential equation
K App c + K V pp_ c =
f (4.32)
with pc (0) = 0, K Ap = kAp I and K V p = kV p I . It is worth pointing out that
pc resulting from integration of (4.32) provides an integral control action on
the force error.
Projecting (4.32) on nc gives
kAp pc;n + kV pp_c;n =
fn, (4.33)
f n p
while assuming that d is along c , it is c;t = 0 and _ c;t = 0. On the other p
hand, Equation (4.31) with (2.30) reveals that the end-effector position e p
p p p
tracks the trajectory given by r , i.e. e = r and _ e = _ r . Therefore, p p
projecting (4.11) and (4.29) on the contact plane gives
pe;t = pd;t (4.34)
p_ e;t = p_ d;t (4.35)
 
f
q
q_
pe

p_
e

Direct Force Control 75

p p_ p
d d d

p p f
f p_ p_ a
c r

d FORCE PARALLEL POSITION p INVERSE  MANIPULATOR q

p p
c r
CONTROL
c
COMPOSITION
r
CONTROL DYNAMICS & ENVIRONMENT q_

p
p_
e
DIRECT
e KINEMATICS

Figure 4.6. Force and position control with full parallel composition.

and thus tracking of the desired end-effector position along the unconstrained
task directions is ensured.
p
Further, assuming that the component of d along c is constant, the pro- n
jection of (4.29) and (4.30) on the normal to the contact plane gives
p_e;n = p_c;n (4.36)
pe;n = pc;n. (4.37)
Folding (4.36) and (4.37) into (4.33) and using (4.24) with constant fd;n yields
?1 fn + kV p k?1 f_n =
fn
kApkf;n f;n (4.38)
which implies regulation of the contact force to the desired value along the
constrained task direction, for any choice of kAp ; kV p > 0. The force error
dynamics continues to depend on the contact stiffness but, remarkably, is now
independent of the position error dynamics, so as wished.
A block diagram of the resulting force and position control scheme is
sketched in Fig. 4.6, where the full parallel composition is evidenced. Com-
pared to the previous scheme in Fig. 4.5, the force control generates the velocity
and acceleration of the origin of the compliant frame as in (4.32) in addition to
its position; these are summed to the corresponding desired position, velocity
and acceleration to provide the analogous quantities to be input to the position
control.

2.3 MOMENT AND ORIENTATION CONTROL

The strategy presented above for force and position control can be con-
ceptually pursued also for moment and orientation control, as shown in the
following.
The counterpart of (4.31) is given by the angular acceleration
ao = !_ r + K Do(!r ? !e) + K PoReere (4.39)
76 ROBOT FORCE CONTROL


where e re is the vector part of the quaternion Qre = Q? e  Qr expressing
1

! !
the orientation error between r and e ; accordingly, r and _ r respectively
denote the angular velocity and acceleration of r .
By analogy with (4.11), (4.29) and (4.30), the parallel composition rule can
be written in terms of

Qr = Qc  Qdc (4.40)
c! = c! + c!
r c dc (4.41)
c !_ = c !_ + c !_
r c dc (4.42)

! !
where Qc , c and _ c characterize the rotational motion of the compliant
! !
frame c , Qdc , dc and _ dc represent the desired rotational motion where the
double subscript indicates that such a motion shall be assigned as a relative
rotation with respect to the time-varying compliant frame; also, the quantities
in (4.41) and (4.42) have been conveniently referred to c .
The analogy is completed by computing the rotational motion of c accord-
ing to the differential equation

K Aoc!_ c + K V oc!c =
c (4.43)

with
 as in (4.4), where K Ao = kAo I and K V o = kV o I . On the other

hand, the origin pc of c is assumed to be constant. As for the previous force
and position control scheme, Qc resulting from integration of (4.43) provides
an integral control action on the moment error.
A block diagram of the resulting moment and orientation control scheme
with parallel composition is sketched in Fig. 4.7, where rotation matrices
have been conventionally indicated to represent orientation of relevant frames.
This scheme is the counterpart of the force and position control scheme in
Fig. 4.6, where the moment control generates the orientation, angular velocity
and angular acceleration of c as in (4.43); these are combined with the
desired orientation, angular velocity and angular acceleration as in (4.40),
(4.41) and (4.42) to generate the corresponding reference quantities to be input
to the orientation control.
By assuming a frictionless and elastically compliant type of contact, the
model of the environment utilized to analyze the behavior during interaction
with the above control scheme is taken as
c  = cK c! dt,
 e (4.44)

!
where c e dt denotes an infinitesimal angular displacement of e and all the
K
quantities have been referred to c . The rotational contact stiffness c  is a
symmetric and positive semi-definite matrix which is constant when referred
 
f
q
q_
pe

p_
e

Direct Force Control 77

!_ ! R
dc dc dc

R R 
 ! !
c r

d MOMENT c PARALLEL r ORIENTATION a o INVERSE  MANIPULATOR q

CONTROL !_
c
COMPOSITION !_ r
CONTROL DYNAMICS & ENVIRONMENT q_

R
!
e
DIRECT
e
KINEMATICS

Figure 4.7. Moment and orientation control with parallel composition.

to c . With reference to Rc in (4.14), a possible choice for cK  is

" #
cK = K ;t 0
c
 (4.45)
0T 0
K
where c ;t is a positive definite matrix expressing the rotational stiffness
about two axes of the plane ( 1c ; 2c ). t t
The elastic contact model (4.44) and (4.45) suggests that, since c n = 0,
a null moment error can be obtained only if the desired moment c d has a 
n
null component along c , i.e. c d;n = 0. Further, a desired rotational motion
n
relative to c can be imposed only about c , i.e. along the unconstrained task
!
direction, and thus the choice c dc;t = 0 and c _ dc;t = 0. These assumptions!
are assumed to hold in the remainder. Accordingly, it is c !c;n = 0 and c !_ c;n = 0
in (4.43) which can thus be projected on the tangential plane, i.e.

kAo c!_ c;t + kV oc! c;t =
ct . (4.46)

Equation (4.39) with (2.31) reveals that the rotational motion of e tracks
! !
the rotational motion of r , i.e. e = r . Therefore, the projection of (4.41)
and (4.42) along c gives n c! = c!
e;n dc;n (4.47)
and thus tracking of the desired end-effector rotational motion along the un-
constrained task direction is ensured.
On the other hand, the projection of (4.41) and (4.42) on the tangential plane
gives
c! = c!
e;t c;t (4.48)
c !_ e;t = c !_ c;t . (4.49)
 p c

p_ c

p c

p d

p_ d

p d

p r

p_ r

p r

a p


f
q
q_
p e

p_ 78e
ROBOT FORCE CONTROL
 d

R c

! c
end−effector path contact force
!_ c
80
!_ dc
0
! dc 60
R dc

R r −0.05 40
!
z [m]

[N]
r

!_ r
z
a o −0.1 20
 x
 0
q −0.15 y
q_ −20
R e
0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
! e
y [m] time [s]

Figure 4.8. Experimental results under force and position regulator with parallel composition.

Computing the time derivative of (4.44) and projecting on the tangential plane
yields
c _ = c ;t c e;t .
t  K ! (4.50)

Then, folding (4.48) and (4.49) into (4.46) and using (4.50) leads to

kAo c K ?;t1 c t + kV o cK ?;t1 c_ t =
ct (4.51)

which implies regulation of the contact moment to the desired value along the
constrained task directions, for any choice of kAo ; kV o > 0.

2.4 EXPERIMENTS
The above force and motion control schemes have been tested in experiments
on the six-joint industrial robot described in Section 3. of Chapter 1 endowed
with the force/torque sensor.

First case study: Force and position regulation. Only the inner three joints
are used while the outer three joints are mechanically braked; accordingly, a
three-DOF task is considered.
The environment and the end effector are those described in Subsection 1.2
of Chapter 3. The end effector is placed in the same initial position as for
the previous case studies. The end-effector task is the same as for the case
study in Subsection 1.2 of Chapter 3, while the same force trajectory as in
Subsection 1.3 starts when contact is detected. The gains of the control action
in (4.12) and (4.5) have been chosen as for the first case study of Subsection 1.3.
The results are presented in Fig. 4.8 in terms of the desired (dashed) and the
actual (solid) end-effector path, together with the time history of the desired
(dashed) and the actual (solid) contact force. As above, the approximate
location (dotted) of the surface is illustrated on the plot of the end-effector
p 1
e;

p d

f d

p c

p d

p d

p_ d

p r

a p


f
q
q_
p e

p_ e

f d Direct Force Control 79

p c

p_ c

p c
end−effector path contact force
p d 40
p_ d
0
p d 30
p r

p_ r −0.05 z
p 20
z [m]

[N]
r

a p

 −0.1 10
f x
q 0
q_ −0.15
y
p e
−10
p_ e 0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
 d y [m] time [s]
R c

! c
end−effector path contact force
!_ c
40
!_ dc 0
! dc 30
R dc

R −0.05 z
r
20
!
z [m]

[N]

!_ r

a −0.1 10
o

 x
 0
q −0.15
y
q_ −10
R e
0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
! e
y [m] time [s]

Figure 4.9. Experimental results under force and position control with full parallel composition.

path, while the instant of contact (dotted line) and the instant of the end of the
motion trajectory (dashed line) are evidenced on the plot of the contact force.
It can be recognized that path tracking accuracy is satisfactory during un-
constrained motion, even with a simple PD position control action plus gravity
compensation. On the other hand, during constrained motion, after a transient
the contact force reaches the desired value; the peak on the component along
Zb is due to the nonnull value of end-effector velocity at the contact as well as
to the imposed motion into the surface, whereas the appreciable deviation from
zero of the component along Yb can be imputed to contact friction and local
deformation of the surface resulting from the imposed end-effector motion.
In any case, both components of contact friction force along Xb and Yb are
regulated to zero in view of the integral action on all the components of the
force error, whereas the component along Zb reaches a steady-state value which
guarantees exact force regulation.

Second case study: Force and position control. In view of its expected
better performance, the force and position control with full parallel composition
is considered.
 p_ e

 d

R c

! c

!_ c

!_ dc

! dc

R dc

R r

! r

80 !_ CONTROL
ROBOT FORCE r

a o


 Z c

q ! dc

q_
R e
 d

! e Y c

Figure 4.10. Geometry of contact.

As above, only the inner three joints are used while the outer three joints are
mechanically braked; accordingly, a three-DOF task is considered.
The environment, the end effector and the task are the same as in the previous
case study. The gains of the control action in (4.31) and (4.32) have been set
K IK IK
to Dp = 90 , Pp = 2500 , Ap = 8 and V p = 480 . I K I
The results are presented in the upper part of Fig. 4.9 in terms of the desired
(dashed) and the actual (solid) end-effector path, together with the time history
of the desired (dashed) and the actual (solid) contact force. As above, the
approximate location (dotted) of the surface is illustrated on the plot of the
end-effector path, while the instant of contact (dotted line) and the instant of
the end of the motion trajectory (dashed line) are evidenced on the plot of the
contact force.
It can be recognized that path tracking accuracy is very good during uncon-
strained motion. On the other hand, the response of the contact force is faster
than that with the force and position regulator, in view of the inverse dynamics
compensation. As a consequence, the peak on the contact force along Zb is
greatly reduced and successful regulation to the desired value continues to be
achieved. A smaller deformation of the surface occurs which also contributes
to reducing the contact friction along Yb by a factor of about two.
As for the second case study of impedance control with inner motion control
in Subsection 2.4 of Chapter 3, to investigate robustness of the scheme with
respect to changes in the environment location, the task is repeated with the
same control parameters as above but the cardboard box is raised by about
0:025 m. From the results presented in the lower part of Fig. 4.9, it can be
recognized that, despite the different location of the surface, the desired force
set point is still achieved; however, larger values of contact force are obtained
during the transient due to the larger impact velocity.
 p_ d

p d

p r

p_ r

p r

a p


f
q
q_
p e

p_ e

 d

R c

! c

!_ c
Direct Force Control 81
!_ dc

! dc

R dc
rotation about approach axis contact moment
R r
2.5
! r
0.5
!_ 2
r
y
a o 0.4 1.5



[Nm]
[rad]

0.3 1
q
q_ 0.2
0.5
R e
z
x
0.1
! e 0
Z c
0
Y c −0.5
 d
0 5 10 15 0 5 10 15
! dc
[s] [s]

Figure 4.11. Experimental results under moment and orientation control.

Third case study: Moment and orientation control. All the six joints of
the robot are involved in this case study.
The end effector is that described in Subsection 3.4. The disk is in contact
with a planar surface such that the approach axis is aligned with the normal
to the surface. The contact is characterized by a rotational stiffness matrix
K  = diagf30; 30; 0g Nm/rad, i.e. the unconstrained motion is described by
any rotation about the approach axis. The compliant frame is determined so
that its Zc -axis is aligned with the normal to the surface.
The interaction task is as follows. At t = 2 s, the moment is taken to
[ 0 1:5 0 ]T in c , according to a fifth-order polynomial with null initial
and final first and second time derivatives, and a duration of 0:5 s; the final
value is kept constant for the remaining portion of the task. The desired end-
effector position is kept constant. After a lapse of 6 s, the desired end-effector
orientation is required to make a rotation of 0:5 rad about the approach axis;
the trajectory is generated according to a fifth-order interpolating polynomial
with null initial and final velocities and accelerations, and a duration of 4 s.
The geometry of the contact during the task is depicted in Fig. 4.10 where the
desired angular velocity and contact moment are represented.
A full six-DOF inverse dynamics control is used, where the linear accelera-
tion is taken as a pure position control with the gains in (2.33) as Dp = 65 K I
K I
and Pp = 2500 . On the other hand, the angular acceleration is chosen as
K
in (4.39) and the gains have been set to Do = 65 and Do = 4500 , and I K I
the gains in the moment control action (4.43) have been set to Ao = and K I
K V o = 24 . I
The results are presented in Fig. 4.11 in terms of the time history of the
rotation angle about the approach axis of e and of the three components of the
desired (dashed) and the actual (solid) contact moment. It can be recognized
that satisfactory tracking of the desired end-effector orientation trajectory is
82 ROBOT FORCE CONTROL

achieved, while the contact moment is successfully regulated to the desired

value. Notice that the moment components along Xc and Zc are affected by
contact friction which causes a steady-state deviation on the rotation angle from
the desired value; this cannot be recovered since the integral action operates on
all the components of the moment error, causing an orientation error also about
the unconstrained motion axis.

3. FORCE TRACKING
The common feature of all the above force and motion control schemes is
the possibility of regulating the contact force to a desired value, without using
explicit information on the contact stiffness of the environment. It has been
shown, however, that the actual value of the stiffness does influence the transient
behavior during the interaction. The typical uncertainty on the contact stiffness
constitutes the key limitation preventing force tracking. Below is presented
a force and position control scheme which achieves force tracking along the
constrained task direction and position tracking along the unconstrained task
directions, thanks to the use of an adaptation mechanism on the estimate of the
contact stiffness.

3.1 CONTACT STIFFNESS ADAPTATION

With reference to the force and position control with full parallel composi-
tion, the force control action described in (4.32) can be replaced with

K App c + K V pp_ c =  (4.52)

where
 = kf;n
?1 f
c (4.53)

with
f c = K Apf d + K V pf_ d +
f . (4.54)

It is worth pointing out that a time-varying desired force has been considered
with suitable feedforward of the first and second derivatives in (4.54). Also,
f f f n
d , _ d and  d are assumed to be aligned with c .
Integration of (4.52) gives the position, linear velocity and acceleration of c
to be used in the parallel compositions (4.11), (4.29) and (4.30). Then, the linear
acceleration is computed as in (4.31) for inverse dynamics control.
p n
By assuming that the component of d along c is constant, tracking of the
desired end-effector position along the unconstrained task directions continues
to hold. Moreover, projecting (4.52) on the normal to the contact plane gives

kAp
fn + kV p
f_n +
fn = 0 (4.55)

 Direct Force Control 83

where (4.24), (4.36), (4.37), (4.53) and (4.54) have been used. Equation (4.55)
implies tracking of the desired contact force along the constrained task direc-
tion.
The above result relies on the assumption that kf;n in (4.53) is exactly known,
which is unlikely to be the case in practice. However, the control scheme can
be modified by using an estimate of the contact stiffness to be suitably updated
as shown in the remainder.
Let "^ denote the time-varying estimate of the inverse of the contact stiffness
coefficient
?1 ;
" = kf;n (4.56)
then Equation (4.53) can be modified as
 = "^f c + "^_ (4.57)
where is the solution to the differential equation
_ +  = fc (4.58)
with  > 0 and "^ is computed according to the update law
"^_ = T
f (4.59)
with > 0.
By observing that " in (4.56) is a constant, Equation (4.57) can be rewritten
as
 f f
= " c ? "~ c ? "~_ (4.60)
where
"~ = " ? "^ (4.61)
denotes the estimate error. Then, folding (4.58) into (4.60) gives
 = "f c ? _ ?  (4.62)
where
 = "~ . (4.63)
Therefore, the force control action in (4.52) can be computed with  as in (4.62)
n
in lieu of (4.53). The resulting projection along c gives
kAp
fn + kV p
f_n +
fn = kf;n(_n + n). (4.64)

It can be found that
fn and
_ fn tend to zero as long as the transfer function of
the system described by (4.64) with input n and output
fn is strictly positive
real; a sufficient condition to guarantee this is simply

 < kkV p . (4.65)

Ap
 Z c

Yc

 d

! dc

84 ROBOT FORCE CONTROL

f f_
d d
p p_ p
d d d

p p f
f p_ p_ a
c r

d ADAP FORCE PARALLEL POSITION p INVERSE  MANIPULATOR q

p p
c r
CONTROL
c
COMPOSITION
r
CONTROL DYNAMICS & ENVIRONMENT q_

p
p_
e
DIRECT
e KINEMATICS

Figure 4.12. Force and position control with contact stiffness adaptation.

It can be concluded that tracking of the desired contact force is recovered thanks
to the adaptation on the stiffness coefficient.
A block diagram of the resulting force and position control scheme with
contact stiffness adaptation is sketched in Fig. 4.12. Compared to the scheme
in Fig. 4.6, the derivatives of the desired contact force are input to the force
control described by (4.52), (4.54), (4.57) and (4.58) which is then made
adaptive according to (4.59) to allow for force tracking even with unknown
stiffness.

3.2 EXPERIMENTS
The above force and position control schemes aimed at force tracking have
been tested in experiments on the six-joint industrial robot described in Sec-
tion 3. of Chapter 1 endowed with the force/torque sensor. Only the inner three
joints are used while the outer three joints are mechanically braked.

Case study. The environment and the end effector are those described in
Subsection 1.2 of Chapter 3. A task in the Yb Zb -plane is assigned. An end-
effector displacement of 0:12 m along Zb and of 0:25 along Yb is commanded.
The trajectory along the path is generated according to a fifth-order interpolating
polynomial with null initial and final velocities and accelerations, and a duration
of 6 s after an initial lapse of 2 s. The surface of the cardboard box is nearly
flat and is placed horizontally, i.e. in the Xb Yb -plane. Initially, the end effector
is not in contact with the surface, and a null set point is assigned to the desired
contact force. When the end effector comes in contact with the surface, i.e.
when a nonnegligible force is sensed, the desired force along Zb is taken to 40 N
according to a fifth-order polynomial with null initial and final first and second
time derivatives, and a duration of 1 s. The constant value is kept for 0:5 s, and
then the desired force is taken back to zero in 1 s with the same polynomial as
 R r

! r

!_ r

a o



q
q_
R e

! e

Z c

Y c

 d

! dc

f d

f d Direct Force Control 85

f_ d

p c

p_ c
end−effector path contact force
p c
80
p d
0
p_ d 60
p d

p r −0.05 40
p_
z [m]

[N]
r

p r

a p −0.1 20

f 0
q −0.15 y
q_ −20
p e
0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
p_ e
y [m] [s]

Figure 4.13. Experimental results under force and position control aimed at force tracking.

above, making a tooth-shaped profile. Then, the tooth is replicated three times
with a 0:5 s lapse between each pair of teeth.
The task has first been executed using the force and position control scheme
with feedforward of first and second derivatives of the contact force. The
I
control gains in (4.31) have been set to kDp = 90 and Pp = 2500 . Also, K I
K
the control gains in (4.54) have been set to Ap = 0:005 and V p = 0:15 I K I
so as to achieve a satisfactory behavior with a design value of 15000 N/m for
the stiffness coefficient kf;n .
The experimental results are presented in Fig. 4.13 in terms of the end-
effector path, together with the time history of the desired (dashed) and the
actual (solid) contact force. As usual, the approximate location (dotted) of the
surface is illustrated on the plot of the end-effector path, while the instant of
contact (dotted line) is evidenced on the plot of the contact force. It can be
recognized that a large peak occurs on the contact force along Zb due to the
nonnull velocity at the impact. Also, a time delay is experienced on the tracking
of the reference force trajectory, whereas the appreciable deviation from zero
of the component along Yb is mainly due to contact friction. On the other hand,
the position tracking performance is very good before the contact, whereas after
the contact the end-effector position has to comply with the surface in view of
the imposed desired force.
The above task has been repeated using the control scheme with contact
stiffness adaptation. The values of the control gains are the same as before,
while the control gains in (4.58) and (4.59) have been set to  = 20 and
= 0:0001. The initial estimate has been set so that 1="^(0) = 15000,
according to the above design value.
The experimental results are presented in Fig. 4.14 in terms of the end-
effector path, together with the time history of the desired (dashed) and the
actual (solid) contact force. As usual, the approximate location (dotted) of the
surface is illustrated on the plot of the end-effector path, while the instant of
R r

! r

!_ r

a o



q
q_
R e

! e

Zc

Yc

 d

!dc

fd

f 86
d ROBOT FORCE CONTROL
f_d

p
c

p_
c
end−effector path contact force
p
c
80
p
d
0
p_
d 60
pd

p r −0.05 40
p_
z [m]

[N]
r

pr

pa −0.1 20

f 0
q −0.15 y
q_ −20
p
e
0 0.05 0.1 0.15 0.2 0.25 0 5 10 15 20
p_
e
y [m] [s]

Figure 4.14. Experimental results under force and position control with contact stiffness adap-
tation.

contact (dotted line) is evidenced on the plot of the contact force. It can be
recognized how the adaptation mechanism is capable of reducing the peak and
ensuring good contact force tracking along Zb . This is obtained at the expense
of a larger end-effector position error along Yb due to contact friction, since the
force control action with stiffness adaptation operates on all the task directions.

4. FURTHER READING
Early work on force control can be found in [109]. The integral action for
removing steady-state force errors has traditionally been used; its stability was
proven in [108], while robustness with respect to force measurement delays
was investigated in [104, 112].
The original idea of closing an outer force control loop around an inner
position control loop dates back to [36]. This was a source of inspiration
for the parallel approach to force/position control introduced in [24] using an
inverse dynamics controller. The parallel force and position regulator was
developed in [25]. The moment and orientation control has been derived
in [72]. An experimental analysis of parallel control schemes has been carried
out in [26], while an extensive comparison with the other force and motion
control schemes presented in this chapter can be found in [29]. The extension
of the force and position control scheme to the case of unknown stiffness with
an adaptation mechanism has been proposed in [27] where a complete stability
proof is given, while experimental tests are described in [28]. Another force and
position control scheme with stiffness adaptation has been proposed in [114].
It has been generally recognized that force control may cause unstable be-
haviour during contact with environment. Dynamic models for explaining
this phenomenon were introduced in [40]. Experimental investigations can be
found in [2] and [103]. Emphasis on the problems with a stiff environment is
given in [44]. Modelling of interaction between a manipulator and a dynamic
 Direct Force Control 87

environment was presented in [33], Moreover, control schemes are usually de-
rived on the assumption that the manipulator end effector is in contact with the
environment and this contact is not lost. Impact phenomena may occur which
deserve careful consideration, and there is a need for global analysis of control
schemes including the transition from non-contact into contact and vice-versa,
e.g. [69, 101, 13].
Chapter 5

ADVANCED FORCE AND POSITION CONTROL

The model-based compensation requirements of the direct force control schemes in the
previous chapter can be relaxed by resorting to an adaptive control acting on the estimates
of the dynamic parameters. A regulation scheme and a passivity-based control scheme
are developed with reference to the manipulator dynamics in the task space. Output
feedback versions of the control schemes are derived to cope with the lack of joint velocity
measurements. Experimental results of the advanced force and position control schemes
are presented throughout the chapter.

1. TASK SPACE DYNAMICS

The interaction control schemes presented in the previous two chapters have
been derived on the basis of the dynamic model of the robot manipulator in
the joint space, as presented in Subsection 1.2 of Chapter 2. However, in order
to derive advanced force and position control schemes, it is worth considering
the dynamics directly in the task space. Further, in view of the difficulties
evidenced for analyzing the performance of direct force control schemes aimed
at controlling both contact force and contact moment, the coverage in this
chapter is restricted only to force and position control.
With reference to the Lagrangian dynamics, for a three-joint nonredundant
manipulator (n = m = 3), the components of the end-effector position e p
constitute a set of generalized coordinates for the mechanical system as long as
J
the Jacobian p in (2.6) is nonsingular. Hence, the counterpart of the dynamic
model (2.20) in the task space can be written as
Bp(pe)pe + C p(pe; p_ e)p_ e + gp(pe) = u ? f (5.1)
where B p , C p and g p are related to B , C and g as

Bp = J ?p TBJ ?p 1 (5.2)
89
90 ROBOT FORCE CONTROL
 
C p = J ?p T C ? BJ ?p 1J_ p J ?p 1 (5.3)
gp = J ?p Tg. (5.4)

Notice that friction has been neglected for the sake of simplicity,
u
The vector in (5.1) represents the equivalent end-effector driving forces
which are to be chosen according to some control strategy; then, the joint

driving torques can be computed as

 = J Tp u. (5.5)

A number of useful properties of the dynamic model (5.1) are presented

below which can be conceptually derived from the basic properties holding for
the joint space dynamic model.
B
The inertia matrix p is symmetric and positive definite. If Bm (BM )
denotes its minimum (maximum) eigenvalue, then in a finite region of the task
space it is
O I B p
< Bm  p ( e)  BM I (5.6)
with BM < 1.
The following identity holds
 
p_ Te B_ p(pe; p_ e) ? 2C p(pe; p_ e p_ e = 0. (5.7)

This result is a direct consequence of the so-called passivity property of the

manipulator dynamic model. Further, there exists a choice of matrix p so C
that the matrix

N p(pe; p_ e) = B_ p(pe; p_ e) ? 2C p(pe; p_ e) (5.8)

is skew-symmetric; this also implies that

B_ p(pe; p_ e) = C p(pe; p_ e) + C Tp (pe; p_ e). (5.9)

For this choice, C p satisfies:

C p(pe; v1 )v2 = C p(pe; v2 )v1 (5.10)
C p(pe; v1 + v2 )v3 = C p(pe; v1 )v3 + C p(pe; v2)v3 (5.11)

for all vectors v 1 , v 2 and v 3 .

There exists a constant 0 < CM < 1 so that
kC p(pe; p_ e)k  CM kp_ ek (5.12)

in a finite region of the task space.

 Advanced Force and Position Control 91

The dynamic model (5.1) is linear in the parameters, i.e.

B(pe)pe + C p(pe; p_ e)p_ e + gp(pe) = Y p(pe; p_ e; p e) (5.13)
where Y p is the (3  p) regressor matrix and  is defined as in (2.21). In
particular, the gravity forces can be written as
gp(pe) = Gp(pe)g (5.14)
where Gp is a (3  r ) matrix and  g is an (r  1) vector of dynamic parameters.
Note that r < p since gravity forces depend only on the mass and first moment
of inertia of each link.

2. ADAPTIVE CONTROL
Any model-based compensation to be used by the control schemes presented
in the previous three chapters relies on the exact knowledge of the dynamic pa-
rameters. These are typically determined via an identification procedure which
provides accurate estimates of the parameters as the result of experimental tests
executed on the real robot manipulator. Nevertheless, parameter mismatching
always exists in practice and further adjustments of some parameters would be
required in case of variable payloads. In such cases, it is possible to resort to
an adaptation mechanism which provides an on-line update of the estimates of
the dynamic parameters as a function of the relevant (motion and force) errors.
To this purpose, adaptive versions of the force and position control schemes
are developed in the sequel. First, the static model-based compensation is
considered for force and position regulation. Then, the dynamic model-based
compensation is treated for force regulation and position tracking.

2.1 REGULATION
With reference to the force and position regulation control scheme in Sub-
section 2.1 of Chapter 4, the counterpart of the joint space control law (4.13)
with (4.12), (4.11) and (4.5) can be written in the task space as
Z t
u = kPp
pde ? kDpp_ e + f d + kFp
f + kIp 0
f d& + g^p(pe) (5.15)

where the control gains have all been taken as scalars and the damping action
has been conveniently expressed in terms of the end-effector linear velocity;
g
also, the quantity ^ p denotes the estimate of the gravity forces.
g g
In case of perfect gravity compensation ( ^ p = p ), the end-effector position
and contact force at the equilibrium are described by (4.18) and (4.19) as long
f n
as the desired force d is taken along the normal c to the contact plane.
A stability analysis around the equilibrium is carried out below as a useful
premise to the case of imperfect gravity compensation (with adaptation). Let
e = pe;1 ? pe (5.16)
92 ROBOT FORCE CONTROL

denote the deviation of the end-effector position from the equilibrium position.
Then, by using (4.18), (3.9) and (4.15), Equation (5.16) can be rewritten as
 
e = I ? ncnTc
pde + kf;n
?1
f (5.17)

where the first term is tangential to the contact plane while the second term is
normal to the plane. Then, in view of (5.17), it is

f = kf;n nc nTc e. (5.18)

Moreover, in view of (4.24) and (4.25), the deviation can be expressed as

e =
pde + dnnc (5.19)

representing a modified end-effector position error where dn accounts for the

presence of the environment and the desired force along the constrained task
direction.
Computing the time derivative of (5.16) gives

e_ = ?p_ e. (5.20)

Then, folding (5.15) in (5.1) and accounting for (5.18), (5.19) and (5.20) yields

Bp(pe)e + C p(pe; p_ e)e_ + kDpe_ + kPpe + kFp

0 kf;n nc nT e + kIphnc = 0
c
(5.21)
0
where kFp = 1 + kFp and
Z t 
h = nc
T

f d& ? kPpkIp
?1 dn nc . (5.22)
0

Computing the time derivative of (5.22) and accounting for (5.18) gives

h_ = kf;n nTc e. (5.23)

Equations (5.21) and (5.23) describe the closed-loop system in terms of the
(7  1) state vector
z
= [ _ T T h ]T e e (5.24)
as
z_ = Az (5.25)
with
2
?B?p 1(C p + kDpI ) ?B?p 1(kPpI + kFp
0 kf;n nc nT ) ?kIpB ?1 nc 3
c p
A = 64 I O 0
7
5.
0T kf;n nTc 0
(5.26)
 Advanced Force and Position Control 93

Notice that the origin of the state space represents the equilibrium of the
system (5.25). Stability around this equilibrium can be ascertained as follows.
Consider the Lyapunov function candidate

V = 12 z TPz (5.27)

where
2
Bp B p 0
3

P = 64 Bp (kPp + kDp)I + kFp

0 kf;n nc nTc kIpnc 75 (5.28)
0T kIpnTc ?1
kIpkf;n
with  > 0.
The function V can be lower-bounded as

V  12 [ ke_ k kek ] ?BBm k ?B


M

ke_ k 
M Pp + kDp kek
(5.29)
" 0 #
1 kFpkf;n ?kIp  jnTc ej 
+ 2 [ jnc ej jhj ]
T
?1 jhj ,
?kIp kIpkf;n
where (5.6) has been used. Equation (5.29) reveals that V is positive definite
provided that

kPp > BBM ? kDp

2 2
(5.30)
m
0 .
kIp < kFp (5.31)

Computing the time derivative of V along the trajectories of the system (5.25)
and (5.26), and accounting for (5.7), yields

V_ = ?e_ T (kDpI ? Bp)e_ + eTC Tp e_ ? kPpeTe (5.32)

?(kFp 0 ? kIp )kf;n (nT e)2
c
where (5.9) has been used. In view of (5.6) and (5.12), the function V_ can be
upper-bounded as

V_  ?(kDp ? BM ? CM kek)ke_ k2 ? kPpkek2 ? (kFp

0 ? kIp)kf;n (nT e)2 .
c
(5.33)

Consider the region of the state space

Z = fz : kz k < g. (5.34)

94 ROBOT FORCE CONTROL

Equation (5.33) reveals that V_ is negative semidefinite in Z provided that (5.31)

0 <  < kDp ? BM .

holds and

CM (5.35)

It can be recognized that V_ = 0 implies e_ = e

0 and = 0. Then, in view
of (5.21), in the largest invariant set containing the set where V_ = 0 it is
z
h = 0. Asymptotic stability of = 0 can be concluded from LaSalle theorem,
implying regulation of the contact force along the constrained task direction and
regulation of the end-effector position along the unconstrained task directions
to their respective desired values.
It is worth remarking that the stiffness coefficient kf;n does not appear in
the conditions (5.30), (5.31), which can be easily satisfied since  is a free
parameter not used by the control. Notice, however, that such conditions are
only sufficient for asymptotic stability, i.e. stability may hold even though they
are not satisfied.
g g
In the case of imperfect gravity compensation ( ^ p 6= p ), a different equi-
librium position is reached for the system (5.1) under the control (5.15), while
contact force regulation is still achieved, i.e.
  
p^ e;1 = pe;1 + kPp
?1 I ? nc nT g (^p ) ? g^ (^p )
c p e;1 p e;1 (5.36)
f e;1 = f d (5.37)
showing that an error occurs on the end-effector position along the uncon-
strained task directions which can be reduced by suitably increasing the control
gain kPp . The stability analysis for the closed-loop system is conceptually
analogous to the case of perfect gravity compensation.
An effective way to recover the original equilibrium position is to make the
force and position regulator adaptive with respect to the dynamic parameters
in the gravity forces. By exploiting the linearity property in (5.14), the control
law (5.15) can be rewritten as
u = kPp ? kDpp_ e + f d + kFp
f + Gp(pe)^g (5.38)

where ^ g is the vector of estimated parameters, and the position
Z t
 =
pde + kPp kIp
f d&
? 1
0
(5.39)

has been made. The equations of the closed-loop system then become
z_ = Az +  (5.40)

?B?p 1Gp~ g 3
with 2

=4 0 5, (5.41)
0
 Advanced Force and Position Control 95

where
~ g = g ? ^ g (5.42)
is the parameter estimate error.
Consider the Lyapunov function candidate

V = 21 zTQz + 21 ~ Tg g ~ g (5.43)

where g is a symmetric and positive definite matrix and

2
Bp B p %B pnc 3

Q = 64 Bp (kPp + kDp)I + kFp 0 kf;n nc nT

c (kIp + %kDp )nc 7
5
%nc B p
T (kIp + %kDp )nc
T ? ? 0
kIpkf;n + %(kPpkf;n + kFp)
1 1

(5.44)
with % > 0. Notice that Q coincides with P in (5.28) when % = 0.
Computing the time derivative of V along the trajectories of the system (5.40),
(5.26) and (5.41), and accounting for (5.7), gives
V_ = ?e_ T(kDpI ? Bp)e_ + (eT + %hnTc )C Tp e_ ? kPpeTe (5.45)
?(kFp0 ? kIp ? %kDp )kf;n (nT e)2 ? %kIp h2 + %kf;n e_ T B p nc nT e
c c
+(e_ T + eT + %hnTc )Gp ~ g ? ^_ g g ~ g
T

where (5.9) has been used.

The last two terms in (5.45) suggest selecting the parameter estimate update
as
 G e e
^_ g = ?g 1 Tp ( _ +  + %h c) n (5.46)
so as to cancel them out. Notice that, in view of (5.19) and (5.22), it is worth
choosing the free parameter % as
?1 kIp
% = kPp (5.47)
which allows the adaptive law (5.46) to be computed in terms of physically
measurable quantities (position, velocity and force) already used in the control
law (5.38), i.e.
 G p
^_ g = ??g 1 Tp ( _ e ?  )  (5.48)

with in (5.39).
The function V in (5.43) can be lower-bounded as

V  12 [ ke_ k kek ] ?BB
m =2 ?BM   ke_ k 
M kPp + kDp kek
(5.49)
"
1 0 kf;n
kFp ?kIp ? %kDp #  jnTc ej 
+ 2 [ jnc ej jhj ]
T
?1 jhj
?kIp ? %kDp kIpkf;n
96 ROBOT FORCE CONTROL
" #
Bm =2
+ 12 [ ke_ k jhj ] ?%B
?%BM ke_ k 
?1 0 )
M %(kPp kf;n + kFp jhj
+ 12 ~ Tg g ~ g
where (5.6) has been used.
On the other hand, by using (5.12) and the inequality

e_ TB pncnTc e  12 BM ke_ k2 + kek2

 
, (5.50)

the function V_ in (5.45) can be upper-bounded as

V_  ?(kDp ? BM ? CM kek ? %kf;n BM =2 ? %CM jhj)ke_ k2 (5.51)

?(kPp ? %kf;nBM =2)kek2
0 ? kIp ? %kDp )kf;n (nT e)2 ? %kIph2 .
?(kFp c
Consider the region (5.34) of the state space with

0 <  < 2(k kDp+kkPp )C . (5.52)

Pp Ip M
Equations (5.49) and (5.51) reveal that V is positive definite and V_ is negative
semidefinite in the region (5.34), provided that

2k2
kIp < k Pp
f;n B(M
(5.53)
!)
kDp > max 2B M ? kPp ; BM 2 + kIp kf;n
2

Bm  kPp (5.54)
8 !2 9
< 2k B 2 k k k
k0 > max Ip M ? Pp ; Ip 1 + Dp =
Fp : kPp Bm kf;n  kPp ;
(5.55)

where (5.47) has been used.

z 
Hence, the system (5.40) is stable around = 0 and ~ g = 0. It can be
e e
recognized that V_ = 0 implies _ = 0 and = 0. Then, in view of (5.21), in
the largest invariant set containing the set where V_ = 0 it is h = 0. Asymptotic
z
stability of = 0 can be concluded from LaSalle theorem. Therefore, thanks
to the adaptive law, regulation of the contact force to the desired value and of
the end-effector position to the original equilibrium is recovered.
It is worth remarking that only the asymptotic convergence of to zero is z
ensured by the previous analysis, whereas the parameter estimate error ~ g is 
 Advanced Force and Position Control 97

bounded without necessarily converging to zero. Also, the parameter  is no

longer free but is used in the adaptive law (5.48). Finally, the conditions (5.53),
(5.54) and (5.55) now require a rough estimate of the contact stiffness coeffi-
cient. In particular, for a given choice of kPp , the gain kIp can be computed
from (5.53), hence the gains kDp from (5.54) and kFp 0 from (5.55) sequentially.
Remind, however, that such conditions are only sufficient.

2.2 PASSIVITY-BASED CONTROL

For the motion control problem, it is known that passivity-based control
exhibits enhanced robustness with respect to inverse dynamics control, since it
does not rely on the exact cancellation of nonlinear terms. The resulting control
law is composed of a nonlinear model-based term and a linear compensator
action. It is termed passivity-based because it is chosen so as to meet a
desired energy function for the closed-loop system which preserves the passivity
property of the manipulator dynamic model implied by (5.7).
For the interaction control problem, a passivity-based control is presented
hereafter which is aimed at guaranteeing tracking of the desired end-effector
position and regulation of the contact force to a desired value. This control
is the natural premise to the case of imperfect model compensation (with
adaptation). As in Subsection 2.2 of Chapter 4, the desired force d is takenf
n
along the normal c to the contact plane and the component of the desired
p n
position d along c is constant.
Consider the control law

u = B^ p(pe)r_ + C^ p(pe; p_ e)r + g^ p(pe) + f + 1 (r ? p_ e) + 2  (5.56)

where 1 ; 2 > 0,  as in (5.39) and r is a reference vector given by

r = p_ d + kPp; (5.57)

also in (5.56) the quantities B

^ p , C^ p and g^ p denote the estimates of the respective
quantities in the dynamic model (5.1).
On the assumption of perfect dynamic compensation (B ^ p = B p , C^ p = C p,
g^ p = gp), folding (5.56) into (5.1) gives
B p(pe)_ + C p(pe; p_ e) + 1 + 2 = 0, (5.58)

where
 = r ? p_ e =
p_ de + kPp. (5.59)
In view of (5.19) and (5.22), the vector  in (5.39) can be rewritten as

 = e + kPp?1 kIp hnc . (5.60)

98 ROBOT FORCE CONTROL

Also, in view of (5.59) and (5.60), the time derivative of the end-effector
deviation from the equilibrium position in (5.19) can be written as

e_ =
p_ de =  ? kPpe ? kIphnc. (5.61)

Thus Equation (5.58) becomes

Bp(pe)_ + C p(pe; p_ e) + 1  + 2e + 2 kPp

?1 kIp hnc = 0. (5.62)

Equations (5.62), (5.61) and (5.23) describe the closed-loop system in terms
of the (7  1) state vector

z0 = [ T eT h ]T (5.63)

as
z_ 0 = A0z 0 (5.64)
with
2
?B?p 1 (C p + 1 I ) ?2B?p 1 ?2kPp
?1 kIp B ?1 nc 3
p
A0 = 64 I ?kPpI ?kIpnc 7
5. (5.65)
0T kf;n nTc 0
Notice that the origin of the state space represents the equilibrium of the
system (5.64). Stability around this equilibrium can be ascertained as follows.
Consider the Lyapunov function candidate

V = 12 z0TP 0 z0 (5.66)

with
Bp2
O 0
3
6 O 2 I ?1 kIpnc 7 .
P =4
0 2kPp 5 (5.67)
0 T
2k?1 kIpnT
Pp c
?
2 kIpkf;n1

The function V can be lower-bounded as

1 ?1 kIp
?kPp " #
kek  . (5.68)
1 1
V  2 Bmkk + 2 2[ kek jhj ] ?k?1k k k?1
2
Pp Ip Ip f;n jhj
where (5.6) has been used. Computing the time derivative of V along the
trajectories of the system (5.64), and accounting for (5.8), gives

V_ = ?1 T ? 2 kPpeTe + 2 kPp ?1 kIp kf;n (nT e)2

c (5.69)
?1 k2 h2 ? 2 kIp hnT e.
?2 kPp Ip c
 Advanced Force and Position Control 99

It is easy to see from (5.69) and (5.68) that V is positive definite and V_ is
negative definite provided that

kPp
2
> 43 kIpkf;n (5.70)

z
holds. Therefore, asymptotic stability of 0 = 0 can be concluded, implying
regulation of the contact force along the constrained task direction and tracking
of the end-effector position along the unconstrained task directions to their
respective desired values.
It is worth remarking that condition (5.70) can be satisfied for a suitable
choice of kPp and kIp with a given estimate of the contact stiffness coeffi-
cient kf;n . Also, stability holds for any 1 and 2 which are then available to
meet design specifications on system performance.
Notice that the last term on the right-hand side of (5.56) is not strictly
needed in the controller, and in fact it is possible to prove stability even without
it. Nevertheless, such term will be useful for the observer design to follow in
the next section.
In the case of imperfect model compensation, the passivity-based control
can be made adaptive with respect to the dynamic parameters as follows. By
exploiting the linearity property in (5.13), the control law (5.56) can be rewritten
as
u Y p p rr f r p
= p ( e ; _ e ; ; _ )^ + + 1 ( ? _ e ) + 2  (5.71)

where ^ is the vector of estimated parameters. The equations of the closed-loop
system become
z Az 
_0 = 0 0 + 0 (5.72)
with A0 as in (5.65) and
2
?B?p 1Y p~ 3
0 = 4 0 5 (5.73)
0
where
~ =  ? ^ (5.74)
is the parameter estimate error.
Consider the Lyapunov function candidate

V = 21 z0TP 0 z0 + 12 ~ T~ (5.75)

P
with 0 as in (5.67) and  a symmetric and positive definite matrix.
It is easy to see that choosing the parameter estimate update law as

^_ = ?1Y Tp  (5.76)

100 ROBOT FORCE CONTROL

yields the same time derivative of V as in (5.69), which is negative semidefinite

provided that (5.70) holds. Hence, the system (5.72) is stable around 0 = 0 z
 z
and ~ = 0, and asymptotic stability of 0 can be concluded from Barbalat
lemma. Therefore, thanks to the adaptive law, regulation of the contact force to
the desired value and tracking of the desired end-effector position is recovered.
The passivity-based adaptive control has been derived in the task space.
Since control actions are to be realized by the joint actuators, it is worth carrying
out the implementation in the joint space. By using the relationship between
the dynamics in the joint space and the dynamics in the task space established
by (5.2), (5.3), (5.4) and (5.5), the counterpart of the control law (5.71) in the
joint space is given by

 = Y (q; q_ ; %; %_ )^ + J Tp (q) (f + 1J p(q)(% ? q_ ) + ) (5.77)

where Y is the regressor in the joint space defined in (2.21), J p is the Jacobian
in (2.6), and

% = J ?p 1 (q)r 
(5.78)
%_ = J ?p 1 (q) r_ ? J_ p(q; q_ )% . (5.79)

Further, the counterpart of the adaptive law (5.76) in the joint space is given by

^_ = ?1 Y T(% ? q_ ). (5.80)

2.3 EXPERIMENTS
The above adaptive passivity-based control scheme has been tested in ex-
periments on the six-joint industrial robot described in Section 3. of Chapter 1
endowed with the force/torque sensor; only the second and third joint are
used while the others are mechanically braked. A minimum set of dynamic
parameters for the manipulator is reported in Appendix B.

Case study. The environment and the end effector are those described in
Subsection 1.2 of Chapter 3. The task consists of a straight-line motion in the
Xb Zb -plane with an end-effector (horizontal) displacement of 0:1 m along Xb .
The trajectory along the path is generated according to a trapezoidal velocity
profile with cubic blends and null initial and final velocities and accelerations;
the actual duration of the motion is 3 s after an initial lapse. The surface of
the cardboard box is nearly flat and is placed (vertically) in the Yb Zb -plane in
such a way as to obstruct the desired end-effector motion along Xb . To begin, a
null set point is assigned to the contact force; when the end effector comes into
contact with the environment, i.e. a nonnegligible force is sensed, the desired
 p
R
c

!p
r
d

!_p_
r
d

ap
r
d

p
o
r

p_ r

qp r

qa_
p

R
f!
e

qZ
e

q_Y
c

p
c
e

! p_
d
e

f
dc
d
d
Rfc
d Advanced Force and Position Control 101
!f_
c
d
!p_
c

!_ p_
c
dc
x 10
−3 position error contact force
!p
c
dc 20 30
Rp
c
dc

Rp_
d
r
15 x x
!p
d
r
20
!p_
d
r

ap_
r
10
o
[m]

[N]
r
 10
p
a
r

p
5
q z
z 0
qf_ 0
qR e

q_ ! e
−5 −10
Zp
c
e
0 2 4 6 8 10 0 2 4 6 8 10
Yp_
c time [s] time [s]

e
d

! Figure 5.1. Experimental results under passivity-based force and position control with exact
dc

f d

f parameter estimation.
d

f_
d

p
c

p_
c x 10
−3 position error contact force
p
c
20 30
p
d

p_
d 15 x x
p 20
d

p r
10
p_
[m]

[N]

r
10
pr

ap
5
z z
 0
f 0
q
q_ −5 −10
ep 0 2 4 6 8 10 0 2 4 6 8 10
ep_ time [s] time [s]

Figure 5.2. Experimental results under passivity-based force and position control with inexact
parameter estimation.

force along Xb is taken to 20 N according to the same kind of interpolating

polynomial as for the position, with null initial and final first and second time
derivatives, and an actual duration of 0:6 s. The constant value is kept for
4 s, and then the desired force is taken back to zero in 0:6 s with the same
polynomial as above.
The task has first been executed using the force and position passivity-based
control scheme with exact parameter estimation. The control gains in (5.77)
and (5.39) have been set to 1 = 1700, 2 = 0, kPp = 40 and kIp = 0:1 in
order to guarantee a well-damped behavior during the unconstrained motion,
as well as to satisfy condition (5.70) with the given stiffness estimate during
the constrained motion.
The experimental results are presented in Fig. 5.1 in terms of the time
history of the desired (dashed) and actual (solid) contact forces, and the time
R r

! r

!_ r

a o



q
q_
R e

! e

Zc

Yc

 d

!dc

fd

f 102
d ROBOT FORCE CONTROL
f_d

p
c

p_
c x 10
−3 position error contact force
p
c
20 30
p
d

p_
d 15 x x
p 20
d

p r
10
p_
[m]

[N]
r
10
pr

pa 5
z
 z 0
f 0
q
q_ −5 −10
p
e
0 2 4 6 8 10 0 2 4 6 8 10
p_
e
time [s] time [s]

Figure 5.3. Experimental results under passivity-based force and position control with inexact
parameter estimation and adaptation.

history of the Xb -component and Zb -component of the end-effector position

error. As usual, the approximate location (dotted) of the surface is illustrated
on the plot of the end-effector path, while the instant of contact (dotted line)
is evidenced on the plot of the contact force. It can be recognized that after
a transient the contact force reaches the desired value, thanks to the integral
action; the peak on the component along Xb is due to the nonnull value of the
end-effector velocity at the contact, as well as to the imposed motion, whereas
the appreciable deviation from zero of the component along Zb is mainly due
to contact friction. On the other hand, the position error along Zb is very small,
whereas a significant position error occurs after the contact along Xb that is
caused by the presence of the surface, as well as by the imposed nonnull desired
force.
The above task has been repeated using the passivity-based control scheme
with inexact parameter estimation. The control gains are unchanged.
The experimental results are presented in Fig. 5.2 in terms of the time history
of the desired (dashed) and actual (solid) contact forces, and the time history of
the Xb -component and Zb -component of the end-effector position error. It can
be recognized that the time history of the contact force is practically the same,
while an appreciable position error occurs along Zb .
Finally, the task has been executed using the passivity-based control scheme
with inexact parameter estimation and adaptation. The control gains in (5.77)
and (5.39) are the same as above. Adaptation is performed with respect to
a reduced set of five dynamic parameters which are affected by uncertainty
on the third link; the matrix gain of the adaptive law (5.80) has been set to
I
 = 70 . In order to test the effectiveness of the control scheme in the face of
parameter uncertainty, the estimated dynamic parameters of the third link have
 ! r

!_ r

a o



q
q_
R e

! e

Z c

Y c

 d

! dc

f d

f d Advanced Force and Position Control 103

f_ d

p c

p_ c
−3
x 10 abs value of z pos error
p c
2.5
p d

p_ d 2
p d

p r
1.5
p_ b
[m]
r

p r

a p
1
 c
f 0.5
q
a
q_ 0
p e
0 2 4 6 8 10
p_ e
time [s]

Figure 5.4. Comparison of performance with passivity-based force and position control
schemes.

been modified assuming that a payload of 10 kg mass is concentrated at the

end effector.
The experimental results are presented in Fig. 5.3 in terms of the time history
of the desired (dashed) and actual (solid) contact forces, and the time history
of the Xb -component and Zb -component of the end-effector position error. It
can be recognized that the position error along Zb is successfully recovered by
resorting to the adaptive action on the parameter estimates.
Indeed, the performance of the three control schemes along the unconstrained
task direction can be evaluated in terms of the position error along Zb as
in Fig. 5.4. Thanks to parameter adaptation, the behaviour of the system
with the adaptive control scheme (c) is better than with the control scheme
without adaptation and inexact parameter estimation (b), and it can be compared
favorably with the control scheme using exact parameter estimation (a).

3. OUTPUT FEEDBACK CONTROL

All previous force and position control schemes require full-state feedback
of the contact force and the end-effector position and velocity. The force is
directly measured by using a force sensor mounted at the robot wrist. The
position can be computed from the joint position measurements via the direct
kinematics, whereas the velocity could be computed from the joint velocity
measurements via the differential kinematics of the manipulator. A problem
exists, however, for those robots having only joint encoders or resolvers for
measuring positions, but no tachometers for measuring joint velocities. There-
fore, it is worth devising suitable output feedback control schemes that do
not require velocity measurements. In the remainder, two solutions are pre-
sented which regard the force and position regulation control scheme and the
passivity-based control scheme discussed above.
104 ROBOT FORCE CONTROL

3.1 REGULATION
p
If the end-effector velocity _ e is not available, it is possible to modify the
control law (5.15) as

u = kPp ? kDp + f d + kFp
f ? k + g^p(pe) (5.81)

where  is defined in (5.39), k > 0, while  and  are respectively obtained
from

_ + 1  = 2p_ e (5.82)
_ + 1  = 2
f (5.83)

with 1 ; 2 > 0. The vector  in (5.82) provides an estimate of the end-effector

velocity through a linear observer (lead filter), whereas the vector  in (5.83)
corresponds to a filtering action on the force error. It is worth pointing out that,
with respect to the control law (5.15), the velocity p_ e has been replaced with
the estimate  while the term ?k  has been introduced for stability purposes
as illustrated in the sequel.
By assuming that the initial conditions for (5.82) and (5.83) are assigned as
  f
(0) = 0 and (0) = ?1 1 2
(0), the following equality holds
 = ?1 1 (kf;n ncnTc  + 2
f ). (5.84)

In fact, taking the time derivative of (5.84) and using (5.82), (5.18) and (5.20)
yields

_ = ?kf;n c Tcnn  (5.85)
and it is easy to verify that Equation (5.83) is satisfied by (5.84) and (5.85).
Folding (5.81) in (5.1), and accounting for (5.18), (5.19), (5.20) and (5.39),
yields

B p(pe)e + C p(pe; p_ e)e_ ? kDp + kPpe + h nc = 0 (5.86)

where
h = nTc (kFp
0 kf;n e + kIp hnc ? k ) (5.87)
g g
with h as in (5.22) and ^ p = p .
Computing the time derivative of (5.87), and accounting for (5.23) and
(5.85), gives
n
h_  = kf;n Tc (kFp e e
0 _ + kIp + k ).  (5.88)

Equations (5.86), (5.82) and (5.88) describe the closed-loop system in terms
of the (10  1) state vector

w = [ e_ T eT T h ]T (5.89)
 Advanced Force and Position Control 105
as
w_ = V w (5.90)
with
2
?B?p 1C p ?kPpB?p 1 kDpB?p 1 ?B?p 1nc 3
V = 664 ?I2 I O O
6 0 7
7
O ?1I 0
7.
5
(5.91)

kf;n kFp nc kf;n kIpnc kf;n knTc

0 T T 0
The origin of the state space gives the equilibrium of the system (5.90). Stability
around this equilibrium can be ascertained as follows.
Consider the Lyapunov function candidate

V = 12 wTXw (5.92)

where
2
Bp 1Bp 12B p 0
3
6  B I O
X = 664  1 Bp kPp 0 7
7
1 2 p O 2 kDpI
?1
0
7
5
(5.93)

0T 0T 0T 0 kf;n )?1
(kFp
with
0?1 kIp
1 = kFp (5.94)
?1 kDp .
2 = kPp (5.95)
The function V can be lower-bounded as
1 
B
V  2 [ ke_ k kek ] ? Bm = 2 ?  1 BM

k e
_ k 

1 M kPp kek (5.96)

1 
B
+ 2 [ ke_ k kk ] ?  Bm = 2 ?  1 2 BM

ke_ k 
+ 1 (k0 k )?1 h2
1 2 M 2 kDp kk 2 Fp f;n 
? 1

where (5.6) has been used. Equation (5.96) with (5.94) and (5.95) reveals that
V is positive definite provided that
( )
B k k02
kIp < m2BPp2 Fp min 1; kPp
2
2 kDp
. (5.97)
M
Computing the time derivative of V along the trajectories of the system (5.90)
and (5.91), and accounting for (5.7), gives
V_ = ?1(2 2 ? 1)e_ TBpe_ ? 1kPpeTe (5.98)
?(1 ?2 1 ? 12)kDp T ? 121 TBpe_
+1 (eT + 2 T )C T e_ ,
106 ROBOT FORCE CONTROL

where (5.6) has been used and the choice

?1 kDp kIp
k = 2 kIp = kPp (5.99)

n
has been made in order to cancel out the cross terms ?1 2 T c h and
h kFp n
0?1 k T . In view of (5.6) and (5.12), the function V_ can be upper-
c
bounded as

V_  ? 12 1(2 2Bm ? 2BM ? 2CM kek ? 22 CM kk)ke_ k2 (5.100)

? 12 kDp(1 ?2 1 ? 212 )kk2 ? 1 kPpkek2

? 12 [ ke_ k kk ] ?122 BBm ?1?21 k1BM kke_ kk .
  

1 2 1 M 1 2 Dp
Consider the region of the state space
W = fw : kwk < g. (5.101)
Equation (5.100) with (5.94) and (5.95) reveals that V_ is negative semidefinite
in W provided that
( )
0 min
kIp < kPpkFp 1 Bm
22 kDp ; 1 BM2 (5.102)

kDp Bm ? 2kPp BM .
0 <  < 22( kPp + kDp )CM (5.103)

e e 
It can be recognized that V_ = 0 implies _ = 0, = 0 and = 0. Then, in
view of (5.86), in the largest invariant set containing the set where V_ = 0 it is
w
h = 0. Asymptotic stability of = 0 can be concluded from LaSalle theorem.
Therefore, thanks to the linear observer, regulation of the contact force and of
the end-effector position to their respective desired values is ensured.
It is worth remarking that the expression of the Lyapunov function is more
complex than in the full-state feedback case, since the additional parameters 1
and 2 are no longer free but they need to be related to the control gains as
in (5.94) and (5.95) in order to simplify the proof. Likewise, the gain k is
related to the control gains as in (5.99). Notice that the gains kDp and kPp
0 and kIp are chosen to satisfy (5.97)
can be chosen to satisfy (5.103); then, kFp
and (5.102).

3.2 PASSIVITY-BASED CONTROL

If the end-effector velocity p_ e is not available, it is possible to modify the
control law (5.56) as
u = Bp(pe)r_  + C p(pe;  )r + gp(pe) + f + 1 (r ?  ) + 2 (5.104)
 Advanced Force and Position Control 107

where perfect dynamic compensation has been assumed. Consider also the
nonlinear observer

p^_ e =  ? (kPp + kO )~pe (5.105)

_ = r_  + 2 B?p 1(pe)( ? p~ e) ? kPpkO p~ e (5.106)

with kO > 0, where

r = p_ d + kPp( ? p~ e) (5.107)
 = p^_ e + kPpp~ e. (5.108)


The vector in (5.108) provides an estimate of the end-effector velocity, where
 p
the vectors and ^ e are the states of the observer and

p~ e = p^ e ? pe (5.109)

denotes the observer position estimation error.

p
Notice that the term ?kPp ~ e in (5.107) has been added in order to allow
r
computation of _  in (5.104) and (5.106) as

r_  = p d + kPp(p_ d ? p^_ e) + kIp
f (5.110)

which depends on end-effector position and contact force only, without requir-
ing velocity measurements. A comparison of the control law (5.104) with the
p
control law (5.56) shows that the end-effector velocity _ e has been replaced

with the estimate and accordingly the reference vector with  .r r
Folding (5.104) in (5.1), and accounting for (5.10) and (5.11), gives

Bp(pe)e_  + C p(pe; p_ e)e + 1e + 2 = 1 " ? C p(pe; r )" (5.111)

where

e = r ? p_ e =
p_ de + kPp( ? p~ e) (5.112)

" =  ? p_ e = p~_ e + kPpp~ e. (5.113)

Then, from (5.105) and (5.106) it is

Bp(pe)(_" + kO ") + 2 p~ e = B p(pe)e_  + 2. (5.114)

Also, combining (5.114) with (5.111) gives

Bp(pe)_" + C p(pe; r )" + kO Bp(pe)" ? 1 " + 2 p~ e = (5.115)

?1e ? C p(pe; p_ e)e
where (5.10) and (5.11) have been exploited again.
108 ROBOT FORCE CONTROL

Computing the time derivative of e in (5.19), and using (5.112) and (5.60),
gives
e_ =
p_ de = e ? kPp( ? p~ e). (5.116)

Equations (5.111), (5.115), (5.116), (5.23) and (5.113), with computed as 

in (5.60), describe the closed-loop system in terms of the (13  1) state vector
w0 = [ eT "T eT h p~ Te ]T (5.117)
as
w_ 0 = V 0w0 (5.118)
with
2
?B?p 1 (C p(pe; p_ e) + 1I ) ?B?p 1 (C p(pe; r ) ? 1I )
6
6 ?B?p 1 (C p(pe; p_ e) + 1I ) ?B?p 1 (C p(pe; r ) ? 1I ) ? kO I
V0= 6
6
6
6
I O (5.119)
4 T T
0 0
O I
?2B?1p ?2k?1kIpB?1 nc
Pp p O 3
O 0 ?2B?p 1 777
?kPpI ?kIpnc ?kPpI 777 :
kf;n nTc 0 0T 5
O 0 ?kPpI
Notice that the origin of the state space represents the equilibrium of the
system (5.118). Stability around this equilibrium can be ascertained as follows.
Consider the Lyapunov function candidate

V = 12 w0T X 0w0 (5.120)

with
Bp O
2
O 0 O 3
6 O Bp O 0 O 7
6 7
X =6 O O
0 6
6 2 I ?1 kIp nc
2 kPp O 7
7.
7 (5.121)
6 T
40 0T 2 k?1 kIpnT
Pp c
?1
2 kIpkf;n 7
0T 5
O O O 0 2 I
The function V can be lower-bounded as
"
?k?1k   #
V  12 2 [ kek jhj ] ?k?1k k Ppk?1Ip kjhekj
1
(5.122)
Pp Ip Ip f;n
1 1 1
+ 2 Bm ke k2 + 2 Bm kk2 + 2 2 kp~ ek2
 Advanced Force and Position Control 109

where (5.6) has been used. Equation (5.122) reveals that V is positive definite
provided that condition (5.70) holds.
Computing the time derivative of V along the trajectories of the system
(5.118), and accounting for the passivity property of the matrix in (5.8), gives
V_ = ?1eT e ? 2 kPpeTe + 2 kPp?1 kIp kf;n (nT e)2
c (5.123)
?2kPp kIph ? 2 kIphnc e
? 1 2 2 T

?kO "TBp" + 1"T " ? eT C p(pe; ")r

?"TC (pe; e )(" + p_ e) + kPpTp~ e ? kPpp~ Te p~ e,
where the equality
C p(pe; r )" + C p(pe; p_ e)e = C p(pe; p_ e)" + C p(pe; e )(" + p_ e) (5.124)
has been exploited. In view of (5.6) and (5.12), the function V_ can be upper-
bounded as
V_  ? (1 ? CM (VM + kPpkek + kIpjhj + kPpkp~ ek + k"k)) ke k2 (5.125)
? (kO Bm ? 1 ? CM (VM + kPpkek + kIpjhj + kPpkp~ ek + ke k)) k"k2
?1 kIp kf;n )kek2 ? 2 k?1 k2 h2 ? 2 kIp jhjkek? kPp kp~ k2 ,
?2 (kPp ? kPp Pp Ip e
where (5.39) has been used, and the upper bound on the desired end-effector
velocity
p
k _ d (t)k  VM (5.126)
has been assumed.
Consider the region of the state space
W 0 = fw0 : kw0 k < 0g. (5.127)
Equation (5.125) reveals that V_ is negative definite in W 0 provided that (5.70)
holds and
0 < 0 < minf1 ; 2 g (5.128)
where

1 = C (21 ? CM VM
M kPp + kIp + 1)
(5.129)

m ? 1 ? CM VM .
2 = CO B(2
k
M k + k + 1)
Pp Ip
(5.130)

w
Therefore, asymptotic stability of 0 = 0 can be concluded, implying reg-
ulation of the contact force to the desired value and tracking of the desired
end-effector position, thanks to the nonlinear observer.
110 ROBOT FORCE CONTROL

It is worth remarking that, with respect to the previous full-state passivity-

based control, 1 is used in (5.129) and (5.130), and only 2 is free.
The passivity-based output feedback control has been derived in the task
space. Since control actions are to be realized by the joint actuators, it is
worth carrying out the implementation in the joint space. By using the rela-
tionship between the dynamics in the joint space and the dynamics in the task
space established by (5.2), (5.3), (5.4) and (5.5), the counterpart of the control
law (5.104) in the joint space is given by

 = B(q)% +C (q; q )%+g(q)+J Tp (q) (f + 1 (r ?  ) + 2 ) (5.131)

where J p is the Jacobian in (2.6), and

% = J ?p 1(q)r (5.132)
q = J p (q)
? 1

(5.133)
% = J ?p 1(q) r_  ? J_ p(q; q )% . (5.134)

As for the observer, its equations can be computed directly by (5.105) and
(5.106).

3.3 EXPERIMENTS
The above passivity-based output feedback control scheme has been tested in
experiments on the six-joint industrial robot described in Section 3. of Chapter 1
endowed with the force/torque sensor; only the inner three joints are used while
the outer three joints are mechanically braked.

Case study. The environment and the end effector are those described in
Subsection 1.2 of Chapter 3. The task consists of a straight-line motion in the
Yb Zb -plane with an end-effector (horizontal) displacement of 0:25 m along Yb
and (vertical) displacement of ?0:15 m along Zb . The trajectory along the path
is generated according to a trapezoidal velocity profile with cubic blends and
null initial and final velocities and accelerations, and a duration of 6 s. The
surface of the cardboard box is nearly flat and is placed (horizontally) in the
Xb Yb -plane in such a way as to obstruct the desired end-effector motion. To
begin, a null set point is assigned to the contact force; when the end effector
comes into contact with the environment, the desired force along Zb is taken to
20 N according to the same kind of interpolating polynomial as for the position,
with null initial and final first and second time derivatives, and a duration of
2 s; that value is then kept constant.
The control and observer gains in (5.131), (5.105) and (5.106) have been set
to 1 = 1700, 2 = 100, kPp = 40, kIp = 0:06 and kO = 100 in order to
guarantee a satisfactory behavior during the constrained motion.
 R r

! r

!_ r

a o



q
q_
R e

! e

Z c

Y c

 d

! dc

f d

f d Advanced Force and Position Control 111

f_ d

p c

p_ c
end−effector path contact force
p c
40
p d
0
p_ d 30
p d z
p r −0.05 20
p_
z [m]

[N]
r

p r

a p −0.1 10
 x
f 0
q −0.15 y
q_ −10
p e
0 0.05 0.1 0.15 0.2 0.25 0 2 4 6 8 10
p_ e
y [m] time [s]

Figure 5.5. Experimental results under passivity-based force and position control with nonlin-
ear observer.

The experimental results are illustrated in Fig. 5.5 in terms of the end-effector
path, together with the time history of the desired (dashed) and the actual (solid)
contact force. As above, the approximate location (dotted) of the surface is
illustrated on the plot of the end-effector path, while the instant of contact
(dotted line) is evidenced on the plot of the contact force. It can be recognized
that after a transient the contact force reaches the desired value; the transient
on the component along Zb is due to the nonnull value of end-effector velocity
at the contact as well as to the imposed motion into the surface, whereas the
appreciable deviation from zero of the component along Yb is mainly due to
contact friction. On the other hand, the path is satisfactorily tracked before
the contact as well as after the contact along Xb , whereas significant position
errors occur after the contact along both Yb - and Zb -axes which are caused
by the respective forces along those directions. It can be concluded that the
use of the nonlinear observer does not compromise the tracking and steady-
state performance of the passivity-based control, and thus it represents a valid
solution when joint velocities are not available.

4. FURTHER READING
The task space dynamics of a robot manipulator was originally considered
in [57] in the context of the operational space approach. A detailed analysis
on the bounds of the inertia matrix has recently been presented in [46]. A
set of useful relations for the vector of Coriolis and centrifugal forces is given
in [102] where gravity adaptation is performed. The property of linearity in
the dynamic parameters dates back to [75], and its extension to the task space
for passivity-based control has been carried out in [76]. Further material about
dynamic model properties used for control purposes can be found in [49].
112 ROBOT FORCE CONTROL

The adaptive force and position regulation control scheme was introduced
in [91]. Passivity-based control of robot manipulators is surveyed in [79, 12]
as a useful premise for adaptive control which indeed was originally proposed
in [98]. Broader references on passivity-based control of nonlinear systems
are [7, 78]. Robustness of passivity-based control with respect to inverse dy-
namics control was discussed in [3]. The passivity approach has been applied
in [92] to design the force regulation and motion control scheme for a robot
manipulator in contact with a compliant surface; its adaptive version is devel-
oped in [93] together with experimental results. Other adaptive force control
schemes can be found in [89, 63, 8, 113].
Design of observers for motion control of robot manipulators was formulated
in [10, 77] for the regulation problem and in [11] for the tracking control
problem using a passivity approach. Those works are at the basis of the output
feedback control schemes; namely, the regulator in [96] and the passivity-based
control in [97]. Further material about the stability of the force and position
controllers and observers is available in [94]. An output feedback force and
position regulator has also been set forth in [62].
Appendix A
Rigid Body Orientation

For the reader’s convenience, a few basic concepts regarding different representations
for the orientation of a rigid body and their relationship with the body angular velocity
are recalled.

1. ROTATION MATRIX
The location of a rigid body in space is typically described in terms of the
p R
(3  1) position vector and the (3  3) rotation matrix describing the origin
and the orientation of a frame attached to the body with respect to a fixed base
R
frame. The matrix is orthogonal, i.e.
RT R = I (A.1)
I
where is the (3  3) identity matrix. It follows that the transpose of a rotation
matrix is equal to its inverse, i.e.
R T = R ?1 . (A.2)

The body linear velocity is described by the time derivative of the position
p
vector, i.e. _ , while its angular velocity !
can be defined through the time
derivative of the rotation matrix by the relationship
R_ = S (!)R; (A.3)
S
where (
) is the operator performing the cross product between two (3  1)
! S!
vectors. Given = [ !x !y !z ]T , ( ) takes on the form
2
0 ?!z !y 3
S (!) = 4 !z 0 ?!x 5 . (A.4)
?!y !x 0
113
114 ROBOT FORCE CONTROL

It is easy to see that S is a skew-symmetric matrix, i.e.

S T(!) = ?S(!); (A.5)

also, it can be shown that

RS (!)RT = S (R!). (A.6)

R
Consider now two frames, conventionally labeled 1 and 2 . Let 1 denote
the rotation matrix expressing the orientation of 1 with respect to the base
R
frame, and 1 2 the rotation matrix expressing the orientation of 2 with respect
to 1 . Then, the orientation of 2 with respect to the base frame is obtained
by composing the successive changes of orientation with respect to the current
frame, i.e.
R
2 = 1
1
R R
2. (A.7)
As usual, a superscript denotes the frame to which a quantity (vector or matrix)
is referred; the superscript is dropped whenever a quantity is referred to the
base frame. From (A.7), the mutual orientation between the two frames can be
described by the rotation matrix
1
R2 = RT1R2. (A.8)

Differentiating (A.8) with respect to time gives

 
1
R_ 2 = RT1 S T(!1) + S(!2 ) R2 (A.9)
= RT1 S (! 2 ? ! 1 )R1 RT1 R2
= S (
1 !21 )1 R2
where (A.5) and (A.6) have been exploited. The quantity

!21 = ! 2 ? ! 1 (A.10)

in (A.9) is the angular velocity of 2 relative to 1 ; the operator
has been

introduced to denote that a vector difference has been taken.

2. EULER ANGLES
Condition (A.1) implies that the nine elements of a rotation matrix are
not independent but related by six constraints. A minimal representation of
'
orientation can be obtained by using a set of three angles = [   ]T .
Consider the rotation matrix expressing the elementary rotation about one of the
coordinate axes as a function of a single angle. Then, a generic rotation matrix
 Appendix A: Rigid Body Orientation 115

can be obtained by composing a suitable sequence of three elementary rotations

while guaranteeing that two successive rotations are not made about parallel
axes. This implies that 12 sets of angles are allowed out of all 27 possible
combinations; each set represents a triplet of Euler angles. For instance, the
XYZ representation of orientation in terms of Euler angles is described by the
rotation matrix

R(') = R2 x()Ry ( )Rz ( ) (A.11)

c c ?c s s 3
= 4 s s c + c s ?s s s + c c ?s c 5
?cs c + ss c s s + sc c c
R R R
where x , y , z are the matrices of the elementary rotations about the three
coordinate axes, and the notations c and s are the abbreviations for cos  and
sin , respectively.
The set of the Euler angles corresponding to a given rotation matrix
2
R11 R12 R13 3
R = 4 R21 R22 R23 5 (A.12)
R31 R32 R33
is

 = Atan2(?R23 ; R33 ) 
q
 = Atan2 R13 ; R11 + R12
2 2 (A.13)
= Atan2(?R12 ; R11 )
with  2 (?=2; =2), whereas the solution is
 = Atan2(R23 ; ?R33 ) 
q
 = Atan2 R13 ; ? R11 + R12
2 2 (A.14)
= Atan2(R12 ; ?R11 )
with  2 (=2; 3=2); the function Atan2(y; x) computes the arctangent of
the ratio y=x but utilizes the sign of each argument to determine which quadrant
the resulting angle belongs to.
Solutions (A.13) and (A.14) degenerate when  = =2; in this case, it is
possible to determine only the sum or difference of  and , i.e.

  = Atan2(R21 ; R22 ) (A.15)

where the plus sign applies for  = +=2 and the minus sign applies for
 = ?=2. These configurations are termed representation singularities.
116 ROBOT FORCE CONTROL

The relationship between the time derivative of the Euler angles '_ and the
!
body angular velocity is given by

! = T (')'_ , (A.16)

where the transformation matrix T corresponding to the above XYZ represen-

tation is
2
1 0 s 3
T (') = 4 0 c ?sc 5 . (A.17)
0 s c c
T
It can be recognized that becomes singular at the representation singularities
 = =2; notice that, in these configurations, it is not possible to describe an
arbitrary angular velocity with a set of Euler angles time derivatives.
Another set of Euler angles which is widely used in robot kinematics is the
ZYZ representation, corresponding to the three joint angles of a spherical wrist.
The resulting transformation matrix in (A.16) is
2
0 ?s c s 3
T (') = 4 0 c ss 5 (A.18)
1 0 c
which becomes singular at the representation singularities  = 0;  . For the
other ten representations of Euler angles, it can easily be found that the second
angle of the sequence always generates a pair of representation singularities
which are either those of (A.17) or those of (A.18). In other words, no matter
what representation is used, a representation singularity occurs whenever the
first and last axes of rotation in the sequence lie along the same direction.
With reference to the problem of describing mutual orientation between two
frames, the use of Euler angles leads to considering the vector difference

'21 = '2 ? '1 (A.19)

where '1 and '2 are the set of Euler angles that can be extracted from R1 and
R2, respectively.
Alternatively, the mutual rotation matrix 1 R2 can be computed first; then, a
set of Euler angles '21 can be extracted from 1 R2 . The angular velocity
1 ! 21
in (A.9) is related to the time derivative of '21 as

1 !21 = T ('21 )'_ 21 (A.20)

where (A.16) and (A.3) have been used.

 Appendix A: Rigid Body Orientation 117

3. ANGLE/AXIS
An alternative representation of orientation can be obtained in terms of a
rotation angle # about an axis in space described by the (3  1) unit vector . r
The rotation matrix corresponding to an angle/axis representation is
 
R(#; r) = rrT + cos # I ? rrT + sin #S (r). (A.21)

R r R r
It is clear that (?#; ? ) = (#; ), i.e. a rotation by ?# about ? r
r
cannot be distinguished from a rotation by # about ; hence, the angle/axis
representation is not unique.
On the other hand, the angle/axis corresponding to a given rotation ma-
trix (A.12) is

# = cos?1 R11 + R222+ R33 ? 1

 
(A.22)

1
2
R32 ? R23 3
r = 2 sin # 4 R13 ? R31 5 . (A.23)
R21 ? R12
In the case sin # = 0, if R11 + R22 + R33 = 3, then # = 0; this means that no
r
rotation has occurred and is arbitrary (representation singularity). Instead, if
R11 + R22 + R33 = ?1, then # =  and the expression of the rotation matrix
becomes
2rx ? 1 2rx ry 2rx rz
2 2 3

R(; r) = 2rx ry 2ry2 ? 1 2ry rz 5 .

4 (A.24)
2rx rz 2ry rz 2rz2 ? 1
r
The three components rx , ry and rz of the unit vector can be computed by
taking any row or column; for instance, from the first column it is
s

rx =  R112+ 1
ry = R2r12 (A.25)
x
R
rz = 2r .
13
x
However, if rx  0, then the computation of ry and rz is ill-conditioned. In
that case, it is better to use another column to compute either ry or rz , and so
forth.
The relationship between the time derivative of the angle/axis parameters
and the body angular velocity is given by:
#_ = rT ! (A.26)
118 ROBOT FORCE CONTROL
 
1 #
r_ = 2 (I ? rr ) cot 2 ? S (r) !.
T
(A.27)

Notice that Equations (A.26) and (A.27) become ill-defined at the representation
singularity (# = 0).

4. QUATERNION
The drawbacks of the angle/axis representation can be overcome by a four-
parameter representation; namely, the unit quaternion, viz. Euler parameters,
defined as:
Q = f; g  (A.28)
where

 = cos #2 (A.29)

 = sin #2 r, (A.30)

with   0 for # 2 [?;  ];  is called the scalar part of the quaternion while
is called the vector part of the quaternion. They are constrained by
2 + T  = 1, (A.31)
hence the name unit quaternion. It is worth remarking that, differently from the
r
angle/axis representation, a rotation by ?# about ? gives the same quater-
r
nion as that associated with a rotation by # about ; this solves the above
nonuniqueness problem. Also, no singularity occurs.
R
The quaternion extracted from ?1 is denoted as Q?1 and can be computed
as
Q?1 = f; ? g.  (A.32)

The rotation matrix corresponding to a given quaternion is

R(; ) = (2 ? T)I + 2T + 2S (). (A.33)

On the other hand, the quaternion corresponding to a given rotation ma-

trix (A.12) is

 = 12 R11 + R22 + R33 + 1

p
(A.34)
2 p
sgn(R32 ? R23 ) R11 ? R22 ? R33 + 1 3
 = 12 664 sgn(R13 ? R31 )pR22 ? R33 ? R11 + 1 775 . (A.35)
p
sgn(R21 ? R12 ) R33 ? R11 ? R22 + 1
 Appendix A: Rigid Body Orientation 119

The relationship between the time derivative of the Euler parameters and the
!
body angular velocity is established by the so-called propagation rule:

_ = ? 12 T ! (A.36)

_ = 12 E(; )! (A.37)

with
E(; ) = I ? S (). (A.38)

With reference to the problem of describing mutual orientation between two

frames, the quantity
Q21 = f21 ; 1 21 g  (A.39)
R
denotes the quaternion that can be extracted directly from 1 2 . Notice that the

vector part of the quaternion is the same when referred to 2 , i.e. 1 21 = 2 21 . 
The composition rule in terms of quaternions corresponding to (A.7) is
defined by the operator “” as
Q2 = Q1  Q21 (A.40)
with
2 = 1 21 ? T1 1 21 (A.41)
2 = 1 121 + 21 1 + S (1 )1 21 . (A.42)
Note that in (A.40) Q1 and Q2 are the quaternions that can be extracted from
R1 and R2 , respectively, with Q21 as in (A.39).
In view of (A.32) and (A.40), the quaternion expressing the mutual orienta-
tion between two frames can also be computed by the composition
Q21 = Q?1 1  Q2 (A.43)
with
21 = 1 2 + T1 2 (A.44)
1
21 = 1 2 ? 2 1 ? S (1 )2 . (A.45)
The propagation rule (A.36) and (A.37) for the quaternion in (A.39) can be
written as

_21 = ? 12 1T21
1 !21 (A.46)

1
_ 21 = 12 E(21 ; 121 )
1 !21 (A.47)

with E defined as in (A.38).

Appendix B
Models of Robot Manipulators

The kinematic models and dynamic models for the two robot manipulators used in the
experiments are presented.

1. KINEMATIC MODELS
Two industrial robots Comau SMART-3 S are available in the PRISMA
Lab. Each robot manipulator has an open kinematic chain with six revolute
joints, creating an anthropomorphic geometry with nonnull shoulder and elbow
offsets and non-spherical wrist. One manipulator is mounted on a sliding
track (prismatic joint) which provides an additional degree of mobility. The
geometry of the seven-joint manipulator is depicted in Fig. B.1 where the joint
variables are numbered from q0 to q6 . Then, the joint variables for the six-joint
manipulator are numbered from q1 to q6 . Further, the link lengths are denoted
from l0 to l7 and their numerical values, listed in Table B.1, are taken from the
data sheet of the robot manufacturer.

Table B.1. Link lengths for the two robot manipulators.

Link li
0 0:850
1 0:150
2 0:610
3 0:110
4 0:610
5 0:113
6 0:103

121
 O b

X b

Y b

Z b

q1
q2
q3
q4
p e

n e

s e

a e

p ;R d d

p_ ; ! d d

p ; !_ d d

a
 n


q
q_

122 ROBOT FORCE CONTROL

p ;R
p_ ; !
p ;R
p_ ; !
e

d
e

p ; !_ d d

a

f; 
q
q_
p ;R e e

p_ ; ! e e

p ;R d d

p_ ; ! d d

p ; !_ d d

p ;R c c

p_ ; ! c c

p ; !_ c c

a

f; 
q
q_
p ;R e e

p_ ; ! e e


Z b

Y b

Z e

Y e

f ; d d

p ;R c c

a

f; 
q
q_
p ;R e e

p_ ; ! e e

O b

n nT p c c o

?
p o
I ? n nT p c c d

k?1 n nT f
f;n c c d

p 1 e;

p d

f d

p c

p d

p d

p_ d

p r

a p


f
q
q_
p e

p_ e

f d

p c

p_ c

p c

p d

p_ d

p d

p r

p_ r

p r

a p


f
q
q_
p e

p_ e

 d

R c

! c

!_ c

!_ dc

! dc

R dc

R r

! r

!_ r

a o



q
q_
R e

! e

Z c

Y c

 d

! dc

f d

f d

f_ d

p c

p_ c

p c

p d

p_ d

p d

p r

p_ r

p r

a p


f
q
q_
p e

p_ e

Figure B.1. Geometry of seven-joint manipulator.

1.1 SIX-JOINT MANIPULATOR

The kinematic model has been derived using the Denavit and Hartenberg
convention. The computation has been cast in a recursive fashion in terms of
the position and orientation of the intermediate frames attached to the six links
in the chain. Let 2 3
pi;1
pi = 4 pi;2 5 (B.1)
pi;3
denote the position vector of the origin of the frame attached to link i, and
2
Ri;11 Ri;12 Ri;13 3
Ri = 4 Ri;21 Ri;22 Ri;23 5 (B.2)
Ri;31 Ri;32 Ri;33
denote the rotation matrix expressing the orientation of that frame. These
quantities run for i = 1; : : : ; 6 and are assumed to be referred to the base
frame. This is chosen with the Zb -axis aligned with the axis of joint 1 and the
 Appendix B: Models of Robot Manipulators 123

origin at the intersection of the common normal between the axes of joints 1
and 2 with the axis of joint 1.
The sequence is as follows. For link 1:
R1;11 = c1
R1;12 = 0
R1;13 = ?s1
R1;21 = s1
R1;22 = 0 (B.3)
R1;23 = c1
R1;31 = 0
R1;32 = ?1
R1;33 = 0
and
p1;1 = l1 R1;11
p1;2 = l1 R1;21 (B.4)
p1;3 = 0.
For link 2:
R2;11 = c2 R1;11
R2;12 = ?s2 R1;11
R2;13 = ?R1;13
R2;21 = c2 R1;21
R2;22 = ?s2 R1;21 (B.5)
R2;23 = R1;23
R2;31 = s2 R1;32
R2;32 = c2 R1;32
R2;33 = 0
and
p2;1 = l2 R2;11 + p1;1
p2;2 = l2 R2;21 + p1;2 (B.6)
p2;3 = ?l2 R2;31 + p1;3 .
For link 3:
R3;11 = c3 R2;11 + s3 R2;12
124 ROBOT FORCE CONTROL

R3;12 = ?R2;13
R3;13 = c3 R2;12 ? s3 R2;11
R3;21 = c3 R2;21 + s3R2;22
R3;22 = ?R2;23 (B.7)
R3;23 = c3 R2;22 ? s3R2;21
R3;31 = c3 R2;31 + s3R2;32
R3;32 = 0
R3;33 = c3 R2;32 ? s3R2;31
and

p3;1 = l3 R3;11 + p2;1

p3;2 = l3 R3;21 + p2;2 (B.8)
p3;3 = l3 R3;31 + p2;3 .
For link 4:

R4;11 = c4 R3;11 + s4R3;12

R4;12 = R3;13
R4;13 = s4R3;11 ? c4 R3;12
R4;21 = c4 R3;21 + s4 R3;22
R4;22 = R3;23 (B.9)
R4;23 = s4R3;21 ? c4 R3;22
R4;31 = c4 R3;31
R4;32 = R3;33
R4;33 = s4R3;31
and

p4;1 = l4 R3;13 + p3;1

p4;2 = l4 R3;23 + p3;2 (B.10)
p4;3 = l4 R3;33 + p3;3 .
For link 5:

R5;11 = c5 R4;11 + s5 R4;12

R5;12 = ?R4;13
R5;13 = c5 R4;12 ? s5R4;11
R5;21 = c5 R4;21 + s5R4;22
 Appendix B: Models of Robot Manipulators 125

R5;22 = ?R4;23 (B.11)

R5;23 = c5 R4;22 ? s5 R4;21
R5;31 = c5 R4;31 + s5 R4;32
R5;32 = ?R4;33
R5;33 = c5 R4;32 ? s5 R4;31
and

p5;1 = ?l5 R4;13 + p4;1

p5;2 = ?l5 R4;23 + p4;2 (B.12)
p5;3 = ?l5 R4;33 + p4;3 .
For link 6:

R6;11 = c6 R5;11 + s6 R5;12

R6;12 = c6 R5;12 ? s6 R5;11
R6;13 = R5;13
R6;21 = c6 R5;21 + s6 R5;22
R6;22 = c6 R5;22 ? s6 R5;21 (B.13)
R6;23 = R5;23
R6;31 = c6 R5;31 + s6 R5;32
R6;32 = c6 R5;32 ? s6 R5;31
R6;33 = R5;33
and

p6;1 = l6 R5;13 + p5;1

p6;2 = l6 R5;23 + p5;2 (B.14)
p6;3 = l6 R5;33 + p5;3 ,
p p R R
where 6 = e and 6 = e represent the position and the orientation of the
end effector as in (2.2) and (2.3).
J
With reference to (2.6), the Jacobian p is the (3  6) matrix given by:

Jp;11 = ?pe2
Jp;12 = R1;23 re1;3 ? R1;33 re1;2
Jp;13 = R2;23 re2;3 ? R2;33 re2;2
Jp;14 = R3;23 re3;3 ? R3;33 re3;2
Jp;15 = R4;23 re4;3 ? R4;33 re4;2
Jp;16 = R5;23 re5;3 ? R5;33 re5;2
126 ROBOT FORCE CONTROL

Jp;21 = pe1
Jp;22 = R1;33 re1;1 ? R1;13 re1;3
Jp;23 = R2;33 re2;1 ? R2;13 re2;3 (B.15)
Jp;24 = R3;33 re3;1 ? R3;13 re3;3
Jp;25 = R4;33 re4;1 ? R4;13 re4;3
Jp;26 = R5;33 re5;1 ? R5;13 re5;3
Jp;31 = 0
Jp;32 = R1;13 re1;2 ? R1;23 re1;1
Jp;33 = R2;13 re2;2 ? R2;23 re2;1
Jp;34 = R3;13 re3;2 ? R3;23 re3;1
Jp;35 = R4;13 re4;2 ? R4;23 re4;1
Jp;36 = R5;13 re5;2 ? R5;23 re5;1
where
rei = pe ? pi (B.16)
for i = 1; : : : ; 5. The Jacobian J o is the (3  6) matrix given by

Jo;11 = 0
Jo;12 = R1;13
Jo;13 = R2;13
Jo;14 = R3;13
Jo;15 = R4;13
Jo;16 = R5;13
Jo;21 = 0
Jo;22 = R1;23
Jo;23 = R2;23 (B.17)
Jo;24 = R3;23
Jo;25 = R4;23
Jo;26 = R5;23
Jo;31 = 1
Jo;32 = R1;33
Jo;33 = R2;33
Jo;34 = R3;33
Jo;35 = R4;33
Jo;36 = R5;33 .
 Appendix B: Models of Robot Manipulators 127

1.2 SEVEN-JOINT MANIPULATOR

The kinematic model has been derived from that of the six-joint manipulator
as follows.
The base frame is chosen with the Zb -axis aligned with the axis of joint 0,
the Yb -axis aligned with the axis of joint 1, and the origin at the mechanical
limit for the range of the prismatic joint. The orientation and position of the
frame attached to link 0 are given by:
R0;11 = 1
R0;12 = 0
R0;13 = 0
R0;21 = 0
R0;22 = 0 (B.18)
R0;23 = 1
R0;31 = 0
R0;32 = ?1
R0;33 = 0
and
p0;1 = 0
p0;2 = 0 (B.19)
p0;3 = q0.
The orientation and position of the frame attached to link 1 are given by:
R1;11 = c1 R0;11
R1;12 = 0
R1;13 = ?s1 R0;11
R1;21 = 0
R1;22 = ?R0;23 (B.20)
R1;23 = 0
R1;31 = s1 R0;32
R1;32 = 0
R1;33 = c1 R0;32
and
p1;1 = l1c1 R0;11 + p0;1
p1;2 = l0R0;23 + p0;2 (B.21)
p1;3 = l1s1 R0;32 + p0;3 .
128 ROBOT FORCE CONTROL

Then the sequence for the remaining frames proceeds from (B.5) and (B.6)
to (B.13) and (B.14) giving the position and orientation of the end effector.
With reference to (B.15), the additional elements needed for the computation
J
of the (3  7) Jacobian matrix p are:

Jp;10 = R0;13
Jp;20 = R0;23 (B.22)
Jp;30 = R0;33 .
All the remaining elements are the same except Jp;11 , Jp;21 and Jp;31 that are
to be replaced with:

Jp;11 = R0;23 re0;3 ? R0;33 re0;2

Jp;21 = R0;33 re0;1 ? R0;13 re0;3 (B.23)
Jp;31 = R0;13 re0;2 ? R0;23 re0;1
where
re0 = pe ? p0 . (B.24)

With reference to (B.17), the additional elements needed for the computation
J
of the (3  7) Jacobian matrix o are:

Jo;10 = 0
Jo;20 = 0 (B.25)
Jo;30 = 0.
All the remaining elements are the same except Jo;11 , Jo;21 and Jo;31 that are
to be replaced with:

Jo;11 = R0;13
Jo;21 = R0;23 (B.26)
Jo;31 = R0;33 .
2. DYNAMIC MODELS
Since the dynamic parameters are typically not available from the robot
manufacturer, the dynamic models of the two robot manipulators have been
identified in terms of a minimum number of parameters. To this purpose, the
dynamics of the outer three joints has been simply chosen as purely inertial and
decoupled. Only joint viscous friction has been included, since other types of
friction (e.g. Coulomb and dry friction) are difficult to model. With reference
to (2.21), the dynamic parameters are listed in Table B.2.
 Appendix B: Models of Robot Manipulators 129

Table B.2. Identified dynamic parameters for the two robot manipulators.

i Six-joint Seven-joint
1 78:7384 762:5649
2 ?46:8840 114:8766
3 2:2870 45:7747
4 53:5654 ?10:0998
5 ?14:2332 ?69:9282
6 ?9:4194 83:5433
7 3:4727 ?10:1028
8 ?8:5887 994:8749
9 1:2018 119:3700
10 ?0:0989 ?4:6261
11 104:4293 ?9:7233
12 0:2424 ?1:2431
13 0:4411 ?4:0507
14 0:9313 ?1:1146
15 11:5712 133:0195
16 ?5:2824 6:4472
17 3:4585 2:7719
18 3:3329 0:8022
19 35:4644 13:8884
20 0:6165 ?21:1525
21 2:4082 ?7:2273
22 0:6808 4:7310
23 11:7816 ?20:8157
24 0:2077 2:2258
25 3:0460 63:4472
26 0:6165
27 2:4082
28 0:6808
29 11:7816
30 0:2077
31 3:0460

2.1 SIX-JOINT MANIPULATOR

The elements of the symmetric inertia matrix are given by:

B11 = 1 + s22 2 + 2l1 c2 5 + s223 6 + 2s22 8 ? 2l1 s2 10

+2s23 c23 12 + 2c23 (l2 c2 + l1 )16 ? 2s23 (l2 c2 + l1 )17
B12 = ?s2 3 ? c2 9 ? s23 13 ? c23 14
B13 = ?s23 13 ? c23 14
B14 = 0
B15 = 0
130 ROBOT FORCE CONTROL

B16 = 0
B22 = 4 + 15 + 2c3 l216 ? 2s3 l217
B23 = 15 + c3 l2 16 ? s3l2 17 + (1=kr3 )18
B24 = 0
B25 = 0 (B.27)
B26 = 0
B33 = 15 + 18
B34 = 0
B35 = 0
B36 = 0
B44 = 20
B45 = 0
B46 = 0
B55 = 22
B56 = 0
B66 = 24
where the notations ci:::j and si:::j are the abbreviations for cos(qi + : : : + qj )
and sin(qi + : : : + qj ), respectively.
The elements of the matrix C in the Coriolis and centrifugal torques are
given by:

C11 = q_2 s2 c2 2 ? q_2 s2l1 5 + (q_2 + q_3)s23 c23 6 + q_2(c22 ? s22 )8
?q_2l1 c210 + (q_2 + q_3)(c223 ? s223 )12
?(q_2l2 s2c23 + (q_2 + q_3)(l1 + l2 c2)s23 )16
?((q_2 + q_3)(l2 c2 + l1 )c23 ? q_2l2 s2s23)17
C31 = ?q_1 s23 c23 6 ? q_1 (c223 ? s223 )12 + q_1 (l2 c2 + l1)s23 16
+q_1 (l2 c2 + l1 )c23 17
C21 = C31 ? q_1s2 c2 2 + q_1l1 s2 5 ? q_1(c22 ? s22 )8 + q_1 l1 c2 10
+q_1 l2 s2 c23 16 ? q_1 l2 s2 s23 17 (B.28)
C13 = ?C31 ? (q_2 + q_3)c23 13 + (q_2 + q_3 )s23 14
C12 = ?C21 + C13 + C31 ? q_2 3 c2 + q_2 s2 9
C22 = ?q_3 l2 s3 16 ? q_3 l2 c3 17
C32 = q_2l2 s316 + q_2 l2 c3 17
C23 = C22 ? C32
C33 = 0
 Appendix B: Models of Robot Manipulators 131

while Cij = 0 for i; j = 4; 5; 6.

The diagonal matrix of viscous friction coefficients is given by:

F11 = 7
F22 = 11
F33 = 19 (B.29)
F44 = 21
F55 = 23
F66 = 25 .
The vector of gravity torques is given by:

g1 = 0
g3 = g(c23 16 ? s23 17 ) (B.30)
g2 = g3 + g(c2 5 ? s2 10 )
while gi = 0 for i = 4; 5; 6, being g the gravity acceleration.

2.2 SEVEN-JOINT MANIPULATOR

The elements of the symmetric inertia matrix are given by:

B00 = 1 + (c21 + s21 (c2 + 0:5(c22 ? 1)))20

B01 = ?c1 3 + s1 4 ? c1 c213 + c1 s214 ? c1 (l1 + c2 l2 )20
?c1c23 21 + c1 s2322 + s123
B02 = s1 s2 13 + s1 c2 14 + s1s2l2 20 + s1 s23 21 + s1c23 22
B03 = s1s23 21 + s1c23 22
B04 = 0
B05 = 0
B06 = 0
B11 = 2 + (l12 + 0:5l22 (1 + c22 ) + 2l1 l2c2 )20 + 0:5(1 ? c22 )5
+0:5(1 ? c2233 )7 + s22 10 + 2l1 c2 13 ? 2l1 s2 14 + s2233 16
+(l2 c3 + 2l1 c23 + l2 c223 )21 ? (l2 s3 + 2l1 s23 + l2 s223 )22
B12 = ?s211 ? c2 12 ? s23 17 ? c23 18 + s2 l2 23
B13 = ?s23 17 ? c23 18
B14 = 0
B15 = 0 (B.31)
B16 = 0
B22 = 6 + 19 + l22 20 + 2c3 l221 ? 2l2 s3 22
132 ROBOT FORCE CONTROL

B23 = 19 + c3 l2 21 ? s3 l222 + (1=kr3 )24

B24 = 0
B25 = 0
B26 = 0
B33 = 19 + 24
B34 = 0
B35 = 0
B36 = 0
B44 = 26
B45 = 0
B55 = 28
B56 = 0
B66 = 30 .
The elements of the vector c = C q_ of the Coriolis and centrifugal torques
are given by:

c0 = q_12 s1 3 + q_12 c1 4 + ((q_12 + q_22)s1c2 + 2q_1 q_2c1s2 )13

?((q_12 + q_22)s1 s2 ? 2q_1q_2c1 c2 )14
+((q_12 + q_22 )s1 c2 l2 + q_1 (2q_2 c1 s2 l2 + q_1 s1 l1 ))20
+(((q_2 + q_3 )2 + q_12 )s1 c23 + 2q_1 (q_2 + q_3 )c1 s23 )21
+(2q_1 (q_2 + q_3 )c1 c23 ? (q_12 + (q_2 + q_3 )2 )s1 s23 )22 + q_12 c1 23
c1 = q_1 q_2s22 5 + q_1 (q_2 + q_3 )s2233 7 + 2q_1 q_2c22 10 ? q_22 c2 11
+q_22 s2 12 ? 2q_1 q_2 s2 l1 13 ? 2q_1 q_2 l1 c2 14 + 2q_1 (q_2 + q_3 )c2233 16
?(q_2 + q_3)2c23 17 + (q_2 + q_3)2s23 18 ? q_1q_2(2s2 l1 + s22l2 )l2 20
?q_1(q_3s3l2 + 2(q_2 + q_3)s23 l1 + (2q_2 + q_3)s223 l2 )21 (B.32)
?q_1(q_3c3 l2 + 2(q_2 + q_3)(l1 c23 + l2 c223 ))22 + q_2 c2 l223
2

c2 = ?0:5q_12 s22 5 ? 0:5q_12 s2233 7 ? q_12c22 10 + q_12 s2 l1 13

+q_12 c2 l1 14 ? q_12 c2233 16 + q_12 (s2 l1 + 0:5s22 l2 )l2 20
?((2q_2 + q_3)q_3 s3l2 ? q_12 (s23l1 ? s223 l2))21
?((2q_2 + q_3)q_3 c3l2 ? q_12(c23 l1 + c223 l2))22
c3 = ?0:5q_12 s2233 7 ? q_12 c2233 16 + ((0:5q_12 + q_22 )s3l2
+q_12 (s23 l1 + 0:5s223 l2 ))21
+(0:5q_12 + q_22 )c3 l2 + q_12 (c23 l1 + 0:5c223 l2 )22
 Appendix B: Models of Robot Manipulators 133

while ci = 0 for i = 4; 5; 6. The diagonal matrix of viscous friction coefficients

is given by:

F00 = 8
F11 = 9
F22 = 15
F33 = 25 (B.33)
F44 = 27
F55 = 29
F66 = 31 .
The vector of gravity torques is given by:

g0 = 0
g1 = 0 (B.34)
g3 = g(21 c23 ? 22 s23 )
g2 = g3 + g((13 + l2 20 )c2 ? 14 s2)
while gi = 0 for i = 4; 5; 6.
References

[1] An, C.H., Atkeson, C.G., and Hollerbach, J.M. (1988). Model-Based
Control of a Robot Manipulator. MIT Press, Cambridge, MA.

[2] An, C.H. and Hollerbach, J.M. (1989). The role of dynamic models in
Cartesian force control of manipulators. Int. J. of Robotics Research,
8(4):51–72.

[3] Anderson, R.J. (1989). Passive computed torque algorithms for robots.
In Proc. 28th IEEE Conf. on Decision and Control, Tampa, FL, pp. 1638–
1644.

[4] Anderson, R.J. and Spong, M.W. (1988). Hybrid impedance control of
robotic manipulators. IEEE J. of Robotics and Automation, 4:549–556.

[5] Ang, M.H. and Andeen, G.B. (1995). Specifying and achieving passive
compliance based on manipulator structure. IEEE Trans. on Robotics
and Automation, 11:504–515.

[6] Angeles, J. (1997). Fundamentals of Robotic Mechanical Systems.

Springer-Verlag, New York.

[7] Arimoto, S. (1996). Control Theory of Non-linear Mecahnical Systems,

Oxford University Press, Oxford, UK.

[8] Arimoto, S., Liu, Y.H., and Naniwa, T. (1993). Model-based adaptive
hybrid control for geometrically constrained robots. In Proc. 1993 IEEE
Int. Conf. on Robotics and Automation, Atlanta, GA, pp. 618–623.

[9] Atkeson, C.G., An, C.H., and Hollerbach, J.M. (1986). Estimation of
inertial parameters of manipulator loads and links. Int. J. of Robotics
Research, 5(3):101–119.
135
136 ROBOT FORCE CONTROL

[10] Berghuis, H. and Nijmeijer, H. (1993). Global regulation of robots using

only position measurements. Systems & Control Lett., 21:289–293.
[11] Berghuis, H. and Nijmeijer, H. (1993). A passivity approach to
controller–observer design for robots. IEEE Trans. on Robotics and
Automation, 9:740–754.
[12] Brogliato, B., Landau, I.D., and Lozano, R. (1991). Adaptive motion
control of robot manipulators: A unified approach based on passivity.
Int. J. of Robust and Nonlinear Control, 1:187–202.
[13] Brogliato, B., Niculescu, S., and Orhant, P. (1997). On the control of
finite dimensional mechanical systems with unilateral constraints. IEEE
Trans. on Automatic Control, 42:200–215.
[14] Bruni, F., Caccavale, F., Natale, C., and Villani, L. (1996). Experiments
of impedance control on an industrial robot manipulator with joint fric-
tion. In Proc. 5th IEEE Int. Conf. on Control Applications, Dearborn,
MI, pp. 205–210.
[15] Bruyninckx, H., Dumey, S., Dutré, S., and De Schutter, J. (1995). Kine-
matic models for model-based compliant motion in the presence of
uncertainty. Int. J. of Robotics Research, 14:465–482.
[16] Caccavale, F. and Chiacchio, P. (1994). Identification of dynamic param-
eters and feedforward control for a conventional industrial manipulator.
Control Engineering Practice, 2:1039–1050.
[17] Caccavale, F., Natale, C., Siciliano, B., and Villani, L. (1997). Experi-
ments of spatial impedance control. In Experimental Robotics V, Casals,
A. and de Almeida, A.T. (Eds.), Springer-Verlag, London, pp. 93–104.
[18] Caccavale F., Natale, C., Siciliano, B., and Villani, L. (1998). Resolved-
acceleration control of robot manipulators: A critical review with exper-
iments. Robotica, 16:565–573.
[19] Caccavale, F., Natale, C., Siciliano, B., and Villani, L. (1999). Six-DOF
impedance control based on angle/axis representations. IEEE Trans. on
Robotics and Automation, 15:289–300.
[20] Caccavale, F., Siciliano, B., and Villani, L. (1998). Quaternion-based
impedance with nondiagonal stiffness for robot manipulators. In Proc.
1998 American Control Conference, Philadelphia, PA, pp. 468–472.
[21] Caccavale, F., Siciliano, B., and Villani, L. (1999). Robot impedance
control with nondiagonal stiffness. IEEE Trans. on Automatic Control,
44:1943–1946.
 References 137

[22] Canudas de Wit, C., and Brogliato, B. (1997). Direct adaptive impedance
control including transition phases. Automatica, 33:643–649.
[23] Canudas de Wit, C., Siciliano, B., and Bastin, G. (Eds.) (1996). Theory
of Robot Control, Springer-Verlag, London.
[24] Chiaverini, S. and Sciavicco, L. (1993). The parallel approach to
force/position control of robotic manipulators. IEEE Trans. on Robotics
and Automation, 9:361–373.
[25] Chiaverini, S., Siciliano, B., and Villani, L. (1994). Force/position reg-
ulation of compliant robot manipulators. IEEE Trans. on Automatic
Control, 39:647–652.
[26] Chiaverini, S., Siciliano, B., and Villani, L. (1996). Parallel
force/position control schemes with experiments on an industrial robot
manipulator. In Prepr. 13th World Congress of IFAC, San Francisco, CA,
vol. A, pp. 25–30.
[27] Chiaverini, S., Siciliano, B., and Villani, L. (1997). An adaptive
force/position control scheme for robot manipulators. Applied Math-
ematics and Computer Science, 7:293–303.
[28] Chiaverini, S., Siciliano, B., and Villani, L. (1998). Force and posi-
tion tracking: Parallel control with stiffness adaptation. IEEE Control
Systems Mag., 18(1):27–33.
[29] Chiaverini, S., Siciliano, B., and Villani, L. (1999). A survey of robot
interaction control schemes with experimental comparison. IEEE/ASME
Trans. on Mechatronics, 4:273–285.
[30] Chou, J.C.K. (1992). Quaternion kinematic and dynamic differential
equations. IEEE Trans. on Robotics and Automation, 8:53–64.
[31] Colbaugh, R., Seraji, H., and Glass, K. (1993). Direct adaptive impe-
dance control of robot manipulators. J. of Robotic Systems, 10:217–248.
[32] Craig, J.J. (1989). Introduction to Robotics: Mechanics and Control. 2nd
Ed., Addison-Wesley, Reading, MA.
[33] De Luca, A. and Manes, C. (1994). Modeling robots in contact with a dy-
namic environment. IEEE Trans. on Robotics and Automation, 10:542–
548.
[34] De Luca, A., Oriolo, G., and Siciliano, B. (1992). Robot redundancy res-
olution at the acceleration level. Laboratory Robotics and Automation,
4:97–106.
138 ROBOT FORCE CONTROL

[35] De Schutter, J., Bruyninckx, H., Zhu, W.-H., and Spong, M.W. (1998).
Force control: A bird’s eye view. In Control Problems in Robotics and
Automation, Siciliano, B. and Valavanis, K.P. (Eds.), Springer-Verlag,
London, pp. 1–17.

[36] De Schutter, J. and Van Brussel, H. (1988). Compliant robot motion II.
A control approach based on external control loops. Int. J. of Robotics
Research, 7(4):18–33.

[37] Dogliani, F., Magnani, G., and Sciavicco, L. (1993). An open architecture
industrial controller. Newsl. of IEEE Robotics and Automation Society,
7(3):19–21.

[38] Dombre, E. and Khalil, W. (1988). Modélisation et Commande des

Robots. Hermès, Paris.

[39] Egeland, O. and Godhavn, J.-M. (1994). Passivity-based adaptive atti-

tude control of a rigid spacecraft. IEEE Trans. on Automatic Control,
39:842–846.

[40] Eppinger, S.D. and Seering, W.P. (1987). Introduction to dynamic mod-
els for robot force control. IEEE Control Systems Mag., 7(2):48–52.

[41] Fasse, E.D. (1997). On the spatial compliance of robotic manipulators.

ASME J. of Dynamic Systems, Measurement, and Control, 119:839–844.

[42] Fasse, E.D. and Broenink, J.F. (1997). A spatial impedance controller
for robotic manipulation. IEEE Trans. on Robotics and Automation,
13:546–556.

[43] Featherstone, R. and Khatib, O. (1997). Load independence of the dy-

namically consistent inverse of the Jacobian matrix. Int. J. of Robotics
Research, 16:168–170.

[44] Ferretti, G., Magnani, G., and Rocco, P. (1995). On the stability of
integral force control in case of contact with stiff surfaces. ASME J. of
Dynamic Systems, Measurement, and Control, 117:547–553.

[45] Gautier, M. and Khalil, W. (1990). Direct calculation of minimum set

of inertial parameters of serial robots. IEEE Trans. on Robotics and
Automation, 6:368–373.

[46] Ghorbel, F., Srinivasan, B., and Spong, M.W. (1998). On the uniform
boundedness of the inertia matrix of serial robot manipulators. J. of
Robotic Systems, 15:17–28.
 References 139

[47] Goldstein, H. (1980). Classical Mechanics. 2nd Ed., Addison-Wesley,

Reading, MA.
[48] Gorinevski, D.M., Formalsky, A.M., and Schneider, A.Yu. (1997). Force
Control of Robotics Systems, CRC Press, Boca Raton, FL.
[49] Grimm, W.M. (1990). Robot non-linearity bounds evaluation techniques
for robust control. Int. J. of Adaptive Control and Signal Processing,
4:501–522.
[50] Hogan, N. (1985). Impedance control: An approach to manipulation:
Parts I—III. ASME J. of Dynamic Systems, Measurement, and Control,
107:1–24.
[51] Hogan, N. (1988). On the stability of manipulators performing contact
tasks. IEEE J. of Robotics and Automation, 4:677–686.
[52] Hollis, R.L., Salcudean, S.E., and Allan, A.P. (1991). A six-degree-of-
freedom magnetically levitated variable compliance fine-motion wrist:
Design, modeling and control. IEEE Trans. on Robotics and Automation,
7:320–333.
[53] Hsu, P., Hauser, J., and Sastry, S. (1898). Dynamic control of redundant
manipulators. J. of Robotic Systems, 6:133–148.
[54] Kazerooni, H. (1990). Contact instability of the direct drive robot when
constrained by a rigid environment. IEEE Trans. on Automatic Control,
35:710–714.
[55] Kazerooni, H., Sheridan, T.B., and Houpt, P.K. (1986). Robust compliant
motion for manipulators, Part I: The fundamental concepts of compliant
motion. IEEE J. of Robotics and Automation, 2:83–92.
[56] Kelly, R., Carelli, R., Amestegui, M., and Ortega, R. (1989). Adaptive
impedance control of robot manipulators,” IASTED Int. J. of Robotics
and Automation, 4(3):134–141.
[57] Khatib, O. (1987). A unified approach for motion and force control
of robot manipulators: The operational space formulation. IEEE J. of
Robotics and Automation, 3:43–53.
[58] Khosla, P.K. and Kanade, T. (1985). Parameter identification of robot
dynamics. In Proc. 24th IEEE Conf. on Decision and Control, Fort
Lauderdale, FL, pp. 1754–1760.
[59] Lin, S.K. (1988). Euler parameters in robot Cartesian control. In Proc.
1988 IEEE Int. Conf. on Robotics and Automation, Philadelphia, PA,
pp. 1676–1681.
140 ROBOT FORCE CONTROL

[60] Lin, S.K. (1989). Singularity of a nonlinear feedback control scheme for
robots. IEEE Trans. on Systems, Man, and Cybernetics, 19:134–139.
[61] Lončarić, J. (1987). Normal forms of stiffness and compliance matrices.
IEEE J. of Robotics and Automation, 3:567–572.
[62] Lorı́a, A. and Ortega, R. (1996). Output feedback force-position regula-
tion of robot manipulators. Automatica, 32:939–943.
[63] Lozano, R. and Brogliato, B. (1992). Adaptive hybrid force-position
control for redundant manipulators. IEEE Trans. on Automatic Control,
37:1501–1505.
[64] Lu, W.-S. and Meng, Q.-H. (1991). Impedance control with adaptation
for robotic manipulators. IEEE Trans. on Robotics and Automation,
7:408–415.
[65] Lu, Z. and Goldenberg, A.A. (1995). Robust impedance control and
force regulation: Theory and experiments. Int. J. of Robotics Research,
14:225–254.
[66] Luh, J.Y.S., Walker, M.W., and Paul, R.P.C. (1980). Resolved-
acceleration control of mechanical manipulators. IEEE Trans. on Au-
tomatic Control, 25:468–474.
[67] McClamroch, N.H. and Wang, D. (1988). Feedback stabilization and
tracking of constrained robots. IEEE Trans. on Automatic Control,
33:419–426.
[68] Mills, J.K. and Goldenberg, A.A. (1989). Force and position control of
manipulators during constrained motion tasks. IEEE Trans. on Robotics
and Automation, 5:30–46.
[69] Mills, J.K. and Lokhorst, D.M. (1993). Control of robotic manipulators
during general task execution: A discontinuous control approach. Int. J.
of Robotics Research, 12:146–163.
[70] Murray, R.M., Li Z., and Sastry, S.S. (1994). A Mathematical Introduc-
tion to Robotic Manipulation, CRC Press, Boca Raton, FL.
[71] Nakamura, Y. (1991). Advanced Robotics: Redundancy and Optimiza-
tion. Addison-Wesley, Reading, MA.
[72] Natale, C., Siciliano, B., and Villani, L. (1998). Control of moment
and orientation for a robot manipulator in contact with a compliant
environment. In Proc. 1998 IEEE Int. Conf. on Robotics and Automation,
Leuven, B, pp. 1755–1760.
 References 141

[73] Natale, C., Siciliano, B., and Villani, L. (1999). Spatial impedance
control of redundant manipulators. In Proc. 1999 IEEE Int. Conf. on
Robotics and Automation, Detroit, MI, pp. 1788–1793.
[74] Nemec, B. and Žlajpah, L. (1998). Implementation of force control on
redundant robot. In Proc. 1998 IEEE/RSJ Int. Conf. on Intelligent Robots
and Systems, Victoria, B.C., CAN, pp. 1314–1320.
[75] Nicolò, F. and Katende, J. (1983). A robust MRAC for industrial robots.
In Proc. 2nd IASTED Int. Symp. on Robotics and Automation, Lugano,
CH, pp. 162–171.
[76] Niemeyer, G. and Slotine, J.-J.E. (1991). Performance in adaptive ma-
nipulator control. Int. J. of Robotics Research, 10:149–161.
[77] Ortega, R., Lorı́a, A., and Kelly, R. (1995). A semiglobally stable out-
put feedback PI2 D regulator for robot manipulators. IEEE Trans. on
Automatic Control, 40:1432–1436.
[78] Ortega, R., Lorı́a, A., Nicklasson, P.J., and Sira-Ramı́rez, H. (1998).
Passivity-based Control of Euler-Lagrange Systems, Springer-Verlag,
London.
[79] Ortega, R. and Spong, M.W. (1989). Adaptive motion control of rigid
robots: a tutorial. Automatica, 25:877–888.
[80] Patterson, T. and Lipkin, H. (1993). Structure of robot compliance. ASME
J. of Mechanical Design, 115:576–580.
[81] Paul, R.P. (1981). Robot Manipulators: Mathematics, Programming, and
Control. MIT Press, Cambridge, MA.
[82] Paul, R. and Shimano, B. (1976). Compliance and control. In Proc. 1976
Joint Automatic Control Conf., San Francisco, CA, pp. 694–699.
[83] Peshkin, M.A. (1990). Programmed compliance for error corrective as-
sembly. IEEE Trans. on Robotics and Automation, 6:473–482.
[84] Raibert, M.H. and Craig, J.J. (1981). Hybrid position/force control of
manipulators. ASME J. of Dynamic Systems, Measurement, and Control,
103:126–133.
[85] Raucent, B., Campion, G., Bastin, G., Samin, J.C., and Willems, P.Y.
(1992). Identification of the barycentric parameters of robot manipula-
tors from external measurements. Automatica, 28:1011–1016.
[86] Roberson, R.E. and Schwertassek, R. (1988). Dynamics of Multibody
Systems. Springer-Verlag, Berlin.
142 ROBOT FORCE CONTROL

[87] Salisbury, J.K. (1980). Active stiffness control of a manipulator in Carte-

sian coordinates. In Proc. 19th IEEE Conf. on Decision and Control,
Albuquerque, NM, pp. 95–100.

[88] Sciavicco, L. and Siciliano, B. (1996). Modeling and Control of Robot

Manipulators. McGraw-Hill, New York.

[89] Seraji, H. (1987). Adaptive force and position control of manipulators.

J. of Robotic Systems, 4:551–578.

[90] Shepperd, S.W. (1978). Quaternion from rotation matrix. AIAA J. of

Guidance and Control, 1:223–224.

[91] Siciliano, B. and Villani, L. (1993). An adaptive force/position regulator

for robot manipulators. Int. J. of Adaptive Control and Signal Processing,
7:389-403.

[92] Siciliano, B. and Villani, L. (1996). A passivity-based approach to

force regulation and motion control of robot manipulators. Automatica,
32:443–447.

[93] Siciliano, B. and Villani, L. (1996). Adaptive compliant control of robot

manipulators. Control Engineering Practice, 4:705–712.

[94] Siciliano, B. and Villani, L. (1997). Design of parallel force/position con-

trollers and observers for robot manipulators. In Modelling and Control
of Mechanical Systems, Astolfi, A. et al. (Eds.), Imperial College Press,
London, pp. 203–218.

[95] Siciliano, B. and Villani, L. (1997). Six-degree-of-freedom impedance

robot control. In Proc. 8th Int. Conf. on Advanced Robotics, Monterey,
CA, pp. 387–392.

[96] Siciliano, B. and Villani, L. (1997). An output feedback parallel

force/position regulator for a robot manipulator in contact with a com-
pliant environment. Systems & Control Lett., 29:295–300.

[97] Siciliano, B. and Villani, L. (1998). Passivity-based interaction controller

and observer for robot manipulators. ASME J. of Dynamic Systems,
Measurement, and Control, 120:516–520.

[98] Slotine, J.-J.E. and Li, W. (1987). On the adaptive control of robot
manipulators. Int. J. of Robotics Research, 6(3):49–59.

[99] Spong, M.W. and Vidyasagar, M. (1989). Robot Dynamics and Control,
Wiley, New York.
 References 143

[100] Takegaki, M. and Arimoto, S. (1981). A new feedback method for dy-
namic control of manipulators. ASME J. of Dynamic Systems, Measure-
ment, and Control, 102:119–125.
[101] Tarn, T.-J., Wu, Y., Xi, N., and Isidori, A. (1996). Force regulation and
contact transition control. IEEE Control Systems Mag., 16(1):32–40.
[102] Tomei, P. (1991) Adaptive PD controller for robot manipulators. IEEE
Trans. on Robotics and Automation, 7:565–5701.
[103] Volpe, R. and Khosla, P. (1993). A theoretical and experimental investi-
gation of impact control for manipulators. Int. J. of Robotics Research,
12:351–365.
[104] Volpe, R. and Khosla, P. (1993). A theoretical and experimental investi-
gation of explicit force control strategies for manipulators. IEEE Trans.
on Automatic Control, 38:1634–1650.
[105] Vukobratović, M. and Nakamura, Y. (1993). Force and contact control
in robotic systems. Tutorial at 1993 IEEE Int. Conf. on Robotics and
Automation, Atlanta, GA.
[106] Wampler, C.W. and Leifer, L.J. (1988). Applications of damped least-
squares methods to resolved-rate and resolved-acceleration control of
manipulators. ASME J. of Dynamic Systems, Measurement, and Control,
110:31–38.
[107] Wen, J.T.-Y. and Kreutz-Delgado, K. (1991). The attitude control prob-
lem. IEEE Trans. on Automatic Control, 36:1148–1162.
[108] Wen, J. and Murphy, S. (1991). Stability analysis of position and force
control for robot arms. IEEE Trans. on Automatic Control, 36:365–371.
[109] Whitney, D.E. (1977). Force feedback control of manipulator fine mo-
tions. ASME J. of Dynamic Systems, Measurement, and Control, 99:91–
97.
[110] Whitney, D.E. (1982). Quasi-static assembly of compliantly supported
rigid parts. ASME J. of Dynamic Systems, Measurement, and Control,
104:65–77.
[111] Whitney, D.E. (1987). Historical perspective and state of the art in robot
force control. Int. J. of Robotics Research, 6(1):3–14.
[112] Wilfinger, L.S., Wen, J.T., and Murphy, S.H. (1994). Integral force
control with robustness enhancement. IEEE Control Systems Mag.,
14(1):31–40.
144 ROBOT FORCE CONTROL

[113] Yao, B. and Tomizuka, M. (1995) Adaptive control of robot manipula-

tors in constrained motion — Controller design. ASME J. of Dynamic
Systems, Measurement, and Control, 117:320–328.
[114] Yao, B. and Tomizuka, M. (1998). Adaptive robust motion and force
tracking control of robot manipulators in contact with compliant surfaces
with unknown stiffness. ASME J. of Dynamic Systems, Measurement,
and Control, 120:232–240.
[115] Yoshikawa, T. (1987). Dynamic hybrid position/force control of robot
manipulators—Description of hand constraints and calculation of joint
driving force. IEEE J. of Robotics and Automation, 3:386–392.
[116] Yoshikawa, T. (1990). Foundations of Robotics. MIT Press, Cambridge,
MA.
[117] Yuan, J.S.-C. (1988). Closed-loop manipulator control using quaternion
feedback. IEEE J. of Robotics and Automation, 4:434–440.
Index

Active compliance, 32 Inner motion control, 38, 41, 67

Active impedance, 37 Inner/outer motion/force control, 3
Adaptive control, 91, 94, 99 Integral control, 67, 74, 76
Alternative Euler angles displacement, 45 Interaction control, 1
Alternative Euler angles error, 27, 46 Interaction with environment, 1
Angle/axis, 117 Internal motion, 23
Angle/axis displacement, 44, 46 Inverse dynamics control, 12
Angle/axis error, 16–17, 28, 49 Jacobian, 9, 89, 125
Angular velocity, 9, 113, 116–117, 119 Joint variables, 8
Compliance, 32 Kinematic control, 12
Compliance control, 2, 31, 35 Kinematic model, 8, 121
Compliant frame, 39, 66, 70, 76 Kinematic singularity, 9
Contact force, 2 Kinetic energy, 10, 26, 43, 47
Contact stiffness, 33, 70, 76, 82 Linear observer, 104
Differential kinematics, 9 Linear velocity, 9, 113
Direct force control, 2, 65 Moment and orientation control, 75, 80
Disturbance rejection, 39
Motion control, 1, 7
Dynamic model, 10, 89, 128
Mutual orientation, 114, 116, 119
Dynamic model properties, 11, 90
Nondiagonal stiffness, 61
Dynamic model-based compensation, 12, 36, 67
Nonlinear observer, 107
Dynamically consistent pseudo-inverse, 24
Open control architecture, 5
End-effector force, 11
Orientation control, 13, 27
End-effector moment, 11
End-effector orientation, 8 Orientation error, 14
End-effector position, 8 Outer force control, 66
Euler angles, 114 Output feedback control, 103
Euler angles displacement, 44 Parallel composition, 70, 73–76
Euler angles error, 14, 27, 46 Parallel control, 3, 70
Experimental apparatus, 4 Passive compliance, 34
Experiments, 34, 40, 51, 68, 78, 84, 100, 110 Passivity-based control, 97, 106
Force and motion control, 69 PD control with gravity compensation, 26
Force and position control, 72, 79, 106 Position control, 13, 27
Force and position regulation, 70, 78, 91, 104 Position error, 14, 40
Force regulation, 65 Position vector, 8, 113
Force tracking, 82 Potential energy, 10, 26–28, 43, 47, 50
Force/torque sensor, 2, 6, 37 Quaternion, 118
Hybrid position/force control, 3 Quaternion displacement, 44, 50
Impedance, 36 Quaternion error, 19, 28, 51
Impedance control, 2, 36, 41 Redundancy, 9
Indirect force control, 2, 31 Redundancy resolution, 23, 59

145
146 ROBOT FORCE CONTROL

Regulation, 26, 66, 91, 104 Static model-based compensation, 26, 32, 66
Remote center of compliance, 34 Stiffness control, 2
Representation singularity, 15, 54, 115, 117 Task geometric consistency, 43–44, 46, 49, 57, 63
Resolved acceleration, 12–13 Task space control, 12
Rotation matrix, 8, 113 Task space dynamics, 89
Rotational impedance, 43–45, 48 Three-DOF impedance control, 40–41
Six-DOF impedance control, 43, 52, 54, 56–57, 59 Tracking control, 12
Six-DOF stiffness, 61 Translational impedance, 40