ArteagaMA Local Stability Springer

Lecture Notes in Electrical Engineering 798
Marco A. Arteaga
Alejandro Gutiérrez-Giles
Javier Pliego-Jiménez
Local Stability
and Ultimate
Boundedness in
the Control of Robot
Manipulators
Lecture Notes in Electrical Engineering
Volume 798
Series Editors
Leopoldo Angrisani, Department of Electrical and Information Technologies Engineering, University of Napoli
Federico II, Naples, Italy
Marco Arteaga, Departament de Control y Robótica, Universidad Nacional Autónoma de México, Coyoacán,
Mexico
Bijaya Ketan Panigrahi, Electrical Engineering, Indian Institute of Technology Delhi, New Delhi, Delhi, India
Samarjit Chakraborty, Fakultät für Elektrotechnik und Informationstechnik, TU München, Munich, Germany
Jiming Chen, Zhejiang University, Hangzhou, Zhejiang, China
Shanben Chen, Materials Science and Engineering, Shanghai Jiao Tong University, Shanghai, China
Tan Kay Chen, Department of Electrical and Computer Engineering, National University of Singapore,
Singapore, Singapore
Rüdiger Dillmann, Humanoids and Intelligent Systems Laboratory, Karlsruhe Institute for Technology,
Karlsruhe, Germany
Haibin Duan, Beijing University of Aeronautics and Astronautics, Beijing, China
Gianluigi Ferrari, Università di Parma, Parma, Italy
Manuel Ferre, Centre for Automation and Robotics CAR (UPM-CSIC), Universidad Politécnica de Madrid,
Madrid, Spain
Sandra Hirche, Department of Electrical Engineering and Information Science, Technische Universität
München, Munich, Germany
Faryar Jabbari, Department of Mechanical and Aerospace Engineering, University of California, Irvine, CA,
USA
Limin Jia, State Key Laboratory of Rail Traffic Control and Safety, Beijing Jiaotong University, Beijing, China
Janusz Kacprzyk, Systems Research Institute, Polish Academy of Sciences, Warsaw, Poland
Alaa Khamis, German University in Egypt El Tagamoa El Khames, New Cairo City, Egypt
Torsten Kroeger, Stanford University, Stanford, CA, USA
Yong Li, Hunan University, Changsha, Hunan, China
Qilian Liang, Department of Electrical Engineering, University of Texas at Arlington, Arlington, TX, USA
Ferran Martín, Departament d’Enginyeria Electrònica, Universitat Autònoma de Barcelona, Bellaterra,
Barcelona, Spain
Tan Cher Ming, College of Engineering, Nanyang Technological University, Singapore, Singapore
Wolfgang Minker, Institute of Information Technology, University of Ulm, Ulm, Germany
Pradeep Misra, Department of Electrical Engineering, Wright State University, Dayton, OH, USA
Sebastian Möller, Quality and Usability Laboratory, TU Berlin, Berlin, Germany
Subhas Mukhopadhyay, School of Engineering & Advanced Technology, Massey University,
Palmerston North, Manawatu-Wanganui, New Zealand
Cun-Zheng Ning, Electrical Engineering, Arizona State University, Tempe, AZ, USA
Toyoaki Nishida, Graduate School of Informatics, Kyoto University, Kyoto, Japan
Federica Pascucci, Dipartimento di Ingegneria, Università degli Studi “Roma Tre”, Rome, Italy
Yong Qin, State Key Laboratory of Rail Traffic Control and Safety, Beijing Jiaotong University, Beijing, China
Gan Woon Seng, School of Electrical & Electronic Engineering, Nanyang Technological University,
Singapore, Singapore
Joachim Speidel, Institute of Telecommunications, Universität Stuttgart, Stuttgart, Germany
Germano Veiga, Campus da FEUP, INESC Porto, Porto, Portugal
Haitao Wu, Academy of Opto-electronics, Chinese Academy of Sciences, Beijing, China
Walter Zamboni, DIEM - Università degli studi di Salerno, Fisciano, Salerno, Italy
Junjie James Zhang, Charlotte, NC, USA
The book series Lecture Notes in Electrical Engineering (LNEE) publishes the
latest developments in Electrical Engineering - quickly, informally and in high
quality. While original research reported in proceedings and monographs has
traditionally formed the core of LNEE, we also encourage authors to submit books
devoted to supporting student education and professional training in the various
fields and applications areas of electrical engineering. The series cover classical and
emerging topics concerning:
• Communication Engineering, Information Theory and Networks
• Electronics Engineering and Microelectronics
• Signal, Image and Speech Processing
• Wireless and Mobile Communication
• Circuits and Systems
• Energy Systems, Power Electronics and Electrical Machines
• Electro-optical Engineering
• Instrumentation Engineering
• Avionics Engineering
• Control Systems
• Internet-of-Things and Cybersecurity
• Biomedical Devices, MEMS and NEMS
For general information about this book series, comments or suggestions, please
contact leontina.dicecco@springer.com.
To submit a proposal or request further information, please contact the
Publishing Editor in your country:
China
Jasmine Dou, Editor (jasmine.dou@springer.com)
India, Japan, Rest of Asia
Swati Meherishi, Editorial Director (Swati.Meherishi@springer.com)
Southeast Asia, Australia, New Zealand
Ramesh Nath Premnath, Editor (ramesh.premnath@springernature.com)
USA, Canada:
Michael Luby, Senior Editor (michael.luby@springer.com)
All other Countries:
Leontina Di Cecco, Senior Editor (leontina.dicecco@springer.com)
** This series is indexed by EI Compendex and Scopus databases. **
More information about this series at https://link.springer.com/bookseries/7818

Marco A. Arteaga · Alejandro Gutiérrez-Giles ·
Local Stability and Ultimate

Boundedness in the Control
of Robot Manipulators
Marco A. Arteaga Alejandro Gutiérrez-Giles
Departamento de Control y Robótica, DIE. Centro de Estudios en Computación
Facultad de Ingeniería Avanzada (CECAv)
UNAM UNAM
Coyoacán, Mexico City, Mexico Mexico City, Mexico
Departamento de Electrónica y
Telecomunicaciones, División de Física
Aplicada
Centro de Investigación Científica y de
Educación Superior de Ensenada
Ensenada, Baja California, Mexico
ISSN 1876-1100 ISSN 1876-1119 (electronic)

Lecture Notes in Electrical Engineering
ISBN 978-3-030-85979-4 ISBN 978-3-030-85980-0 (eBook)
https://doi.org/10.1007/978-3-030-85980-0
© The Editor(s) (if applicable) and The Author(s), under exclusive license to Springer Nature
Switzerland AG 2022
This work is subject to copyright. All rights are solely and exclusively licensed by the Publisher, whether
the whole or part of the material is concerned, specifically the rights of translation, reprinting, reuse
of illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way, and
transmission or information storage and retrieval, electronic adaptation, computer software, or by similar
or dissimilar methodology now known or hereafter developed.
The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication
does not imply, even in the absence of a specific statement, that such names are exempt from the relevant
protective laws and regulations and therefore free for general use.
The publisher, the authors and the editors are safe to assume that the advice and information in this book
are believed to be true and accurate at the date of publication. Neither the publisher nor the authors or
the editors give a warranty, expressed or implied, with respect to the material contained herein or for any
errors or omissions that may have been made. The publisher remains neutral with regard to jurisdictional
claims in published maps and institutional affiliations.
This Springer imprint is published by the registered company Springer Nature Switzerland AG
The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland
To Heidrun,
with everlasting love.
—Marco A. Arteaga
To my beloved family,
—Alejandro Gutiérrez-Giles
To my beloved family,
—Javier Pliego-Jiménez
Preface
There is a wide variety of applications for industrial robot manipulators, such as

welding, painting, coating, gluing, sealing, milling, drilling, grinding, screwing,
wiring, fastening or assembling of devices. To carry out any of the different tasks
a manipulator is supposed to do, the design of a high-performance control law is
advisable. Therefore, the control of robot manipulators has received a lot of attention
for the last decades. Although inherently industrial robots are thought for practical
applications, many researchers have also considered them as high nonlinear systems
whose study from a pure theoretical point of view is quite challenging. Problems like
trajectory tracking, state observation, force control, telemanipulation, etc., are just
some examples of a wide variety of subjects under study. One of the main challenges
always to be considered is global stability. Roughly speaking, it means that no matter
how large the initial error state may be, it ultimately will vanish. In fact, theoreti-
cally the initial error could be just infinite. Achieving this goal may be at the cost of
very complex controllers and/or observers, or at the cost of making many unrealistic
assumptions, such as the perfect knowledge of different robot model parameters.
But, is it really necessary to invest such a huge effort to prove that the initial error
can be infinite for a physical system that is confined in few cubic meters and cannot
move with infinite velocity nor apply infinite forces on the environment? Another
issue is that even though from a theoretical point of view all errors tend to zero
exactly, in practice residual errors arise due to a variety of causes such as sensors
resolution, model inaccuracies or the discretization process for computer implemen-
tation. Although global asymptotic stability is quite desirable, this book is devoted to
the design of control schemes and state observers based on the premise of desirable
local stability. Furthermore, ultimate boundedness is considered as an acceptable
alternative to asymptotic stability.
Part I contains the most basic and well-known concepts of robot manipulators.
On its own, the first part of the book can be used for a postgraduate course on
robot control theory. Chapter 1 provides the technical definition of industrial robot
manipulators, the only kind of systems under study, and makes a summary of the most
employed concepts. The usual categories of robot manipulators are given, and it is
explained why it makes just sense to employ the customary kinematic arrangements.
vii
viii Preface
Chapter 2 deals with the robot’s kinematics, and it comprises the development of basic
rotations among two coordinate frames as well as the definition of homogeneous
transformations. Then, the three kinds of kinematics are described in full detail for
serial links manipulators, i.e. direct kinematics, inverse kinematics and differential
kinematics. By taking advantage of all the previous concepts, Chap. 3 develops the
dynamic model of robot manipulators based on the Euler–Lagrange methodology,
including the description of external forces by means of the Lagrange multipliers.
Furthermore, some of the most useful physical and structural properties of the model
are obtained, which prove to be important for control design. Chapter 4 presents some
basic concepts and results on control theory. It is not aimed at giving a deep insight,
but at making the book self-contained. However, the concepts that are inspected with
more detail are precisely those that give name to this book, i.e. local stability in
the sense of Lyapunov and ultimate boundedness. A rather colloquial development
instead of a strict analytical approach is used for the statement of the main theorems
employed for the design of control and observers schemes. Finally, Chap. 5 provides
the most elemental control laws for robot manipulators.
Part II is devoted to recapitulate more than two decades of experience by the
authors in the development of different control laws and approaches for robot manip-
ulators, as well as pointing out some very well-known schemes that comply with the
main goal of this book, i.e. either local stability or ultimate boundedness. Chapter 6
deals with the fact that most industrial robots are not really equipped with velocity
sensors, while this quantity is necessary for control laws implementation. Therefore,
it is necessary to produce an estimate of joint velocities. Although this can readily
be done by numerical differentiation in a digital computer, it is preferable for many
reasons the design of an observer. Model-based observers of the Luenberger type
are a very common solution even for highly nonlinear systems, but it is explained
how much more simpler options can be used to get just as a good performance as for
model-based solutions without needing at all the robot dynamic model for implemen-
tation. In fact, although theoretically it is possible to obtain a very accurate dynamic
model for industrial robots, in practice this is usually not the case because many
physical parameters are not known perfectly or they are even unknown. Therefore,
Chap. 7 discusses two of the most common solutions to compensate robot models
uncertainties, namely adaptive and robust control. The former makes use of the very
well-known property of model parameters linearity, while the latter deals with the
uncertainties by introducing some extra control terms to the nominal law. Pros and
cons are discussed. Chapter 8 introduces some solutions to robot force control. As
explained before, these are based on the authors’ own experiences, and therefore,
the reader will not find common solutions just as impedance or admittance control.
However, the proposed schemes take into account a wide variety of situations, just like
the possible lack of both velocities and force measurements, accurate robot models or
an implementation without computing inverse kinematics. Chapters 9 and 10 study
the intercommunication of different robot manipulators. The former deals only with
the bilateral teleoperation of two devices, while the latter takes into account three or
more, i.e. robot networks. In both cases, it is assumed that time-varying delays are
present in the communication channels, which constitutes a more realistic as well as
Preface ix
more challenging situation. Also, velocity observers are designed, and for the bilat-
eral teleoperation the concept on delayed kinematic correspondence is introduced in
contrast to the usual concept of transparency when no time delays are considered at
all. One of the key characteristics of all the chapters of Part II is that experimental
results are shown. Much unfortunately, the COVID-19 pandemic that humanity has
endured during the writing of this book kept the National Autonomous University
of Mexico closed, so that instead of using the industrial robots A255 and A465
for some experiments, the also reliable but simpler Geomagic Touch manipulators
were employed. While the outcomes are still valid, these smaller robots are easier to
control, and it is worthy to point this out.
Part III discusses the modeling of three of the robots available at the Laboratory
for Robotics of the School of Engineering of the National Autonomous University
of Mexico. Chapters 11 and 12 study the kinematic and dynamic modeling of the
industrial robots A465 and A255 by CRS Robotics, respectively. While the kine-
matic models can be gotten with high accuracy, the dynamic ones are computed
by making many simplifications. This has to be so because the links geometries
are not symmetric, their masses densities are not uniform, and the manufacturer
provides little information altogether. One of the main drawbacks of using indus-
trial robots for testing control–observer approaches is that sometimes they do not
allow to program them since the manufacturer includes its own control algorithms.
Therefore, Chap. 13 makes a proposal about how some hardware devices can be
built to replace the original control units, thus allowing the implementation of newly
developed control schemes. Finally, Chap. 14 computes the kinematic and dynamic
models of the haptic device Geomagic Touch, employed as mentioned before in all
the experiments of the second part of the book.
Mexico City, Mexico Marco A. Arteaga

Mexico City, Mexico Alejandro Gutiérrez-Giles
Ensenada, Mexico Javier Pliego-Jiménez
June 2021
Acknowledgements The first author would like to thank the support given by the DGAPA–UNAM
under grant IN117820. The second author thanks to the DGAPA–UNAM postdoctoral scholarships
program. The third author thanks to the Cátedra CONACYT 1030.
Contents
Part I Preliminaries
1 A General Overview of Robot Manipulators . . . . . . . . . . . . . . . . . . . . . 3
1.1 Brief History of Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Industrial Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3 Common Kinematic Arrangements . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.3.1 Articulated Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3.2 Spherical Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.3.3 SCARA Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.3.4 Cylindrical Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.5 Cartesian Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.4 Wrists and End-Effectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.4.1 Spherical Wrist . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.4.2 Common End-Effectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.5 Some Other Important Issues to Take into Account . . . . . . . . . . . . 13
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2 Position, Orientation and Velocity of Rigid Robot
Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.1 Rigid Motions and Homogeneous Transformations . . . . . . . . . . . . 15
2.1.1 Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.1.2 Composition of Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.1.3 Different Parametrizations for Rotation Matrices . . . . . . 25
2.1.4 Unit Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
2.1.5 Homogeneous Transformations . . . . . . . . . . . . . . . . . . . . . 31
2.1.6 Skew Symmetric Matrices . . . . . . . . . . . . . . . . . . . . . . . . . 34
2.1.7 Angular Velocity and Acceleration . . . . . . . . . . . . . . . . . . 37
2.2 Direct Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
2.2.1 Kinematic Chains . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
2.2.2 The Denavit-Hartenberg Representation . . . . . . . . . . . . . . 43
2.3 Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
2.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
xi
xii Contents
2.3.2 Kinematic Decoupling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

2.3.3 Inverse Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
2.4 Differential Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
2.4.1 Analytic Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
2.4.2 Geometric Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
2.4.3 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
3 Dynamics of Rigid Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . 71
3.1 Dynamic Modeling of Rigid Robot Manipulators . . . . . . . . . . . . . 71
3.1.1 Euler-Lagrange Equations of Motion . . . . . . . . . . . . . . . . 71
3.1.2 Kinetic Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
3.1.3 Potencial Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
3.2 Equations of Motion of a Robot Manipulator . . . . . . . . . . . . . . . . . 78
3.2.1 Generalized Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
3.3 Inclusion of Environmental Forces . . . . . . . . . . . . . . . . . . . . . . . . . . 86
3.4 Model Properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
3.4.1 Vectors and Matrices Properties . . . . . . . . . . . . . . . . . . . . . 90
3.4.2 Norm Related Properties . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
3.4.3 Whole Model Related Properties . . . . . . . . . . . . . . . . . . . . 96
3.4.4 Holonomic Constraints Properties . . . . . . . . . . . . . . . . . . . 98
3.5 Inclusion of DC Motors in the Robot Dynamic Model . . . . . . . . . 99
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
4 Mathematical Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
4.1 Basic Definitions and Lemmas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
4.2 Stability in the Sense of Lyapunov . . . . . . . . . . . . . . . . . . . . . . . . . . 107
4.2.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
4.2.2 Main Stability Theorem . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
4.2.3 Complementary Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
4.3 Ultimate Boundedness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
4.3.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
4.3.2 An Ultimate Boundedness Theorem . . . . . . . . . . . . . . . . . 121
4.4 Sliding Surfaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
5 Common Control Approaches for Robot Manipulators . . . . . . . . . . . 129
5.1 PD and PD+ Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129
5.2 PID Control of Robot Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . 131
5.3 Computed Torque Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
5.4 Exploiting the Passive Structure of Robot Manipulators . . . . . . . . 136
5.5 Design in Work Space Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . 137
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
Contents xiii
Part II Looking for Semiglobal Stability or Ultimate Boundedness

6 Velocity Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143
6.1 The Nicosia and Tomei Observer . . . . . . . . . . . . . . . . . . . . . . . . . . . 143
6.2 Non Model Based Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . 149
6.3 Non Model Based Observer and Control Design . . . . . . . . . . . . . . 153
6.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164
7 Adaptive and Robust Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
7.1 The Adaptive Law by Slotine and Li . . . . . . . . . . . . . . . . . . . . . . . . 165
7.2 Adaptive Scheme with Velocity Observers . . . . . . . . . . . . . . . . . . . 168
7.3 Robust Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179
7.4 Control-Observer Robust Scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
7.5 Generalized Proportional Integral (GPI) Observer . . . . . . . . . . . . . 187
7.6 GPI Observer Without Inertia Matrix . . . . . . . . . . . . . . . . . . . . . . . . 191
7.7.1 Performance Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . 207
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 212
8 Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215
8.1 Robot Force Control Without Dynamic Model . . . . . . . . . . . . . . . . 215
8.2 Velocity and Force Observers for the Control of Robot
Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 230
8.3 GPI Based Velocity/Force Observer Design for Robot
Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 237
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 255
9 Bilateral Teleoperation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257
9.1 Fundamental Concepts of Bilateral Manipulators Systems . . . . . . 257
9.2 Control and Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 260
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 280
10 Robot Networks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283
10.1 Basic Characteristic of Robot Networks . . . . . . . . . . . . . . . . . . . . . 283
10.2 Leaderless Consensus Problem (LCP) . . . . . . . . . . . . . . . . . . . . . . . 285
10.3.1 Leader-Follower Consensus Problem . . . . . . . . . . . . . . . . 301
References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 310
xiv Contents
Part III Different Testbeds and the Adaptation of Industrial

Robots for Practical Implementation
11 The Robot CRS 465 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313
11.1 Characteristics of the Robot CRS A465 . . . . . . . . . . . . . . . . . . . . . . 313
11.2 Kinematics of the Robot A465 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313
11.3 Dynamics of the Robot A465 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 316
12 The Robot CRS 255 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 329
12.1 Characteristics of the Robot CRS A255 . . . . . . . . . . . . . . . . . . . . . . 329
12.2 Kinematics of the Robot A255 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 329
12.3 Dynamics of the Robot A255 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 332
13 Adapting the Robots CRS 465 and 255 for Original Control
Laws Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 341
13.1 Original System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 341
13.2 Hardware Modification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345
13.2.1 Signal Routing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 346
13.2.2 Digital Stage for the A465 Manipulator . . . . . . . . . . . . . . 346
13.2.3 Digital Stage for the A255 Manipulator . . . . . . . . . . . . . . 350
13.2.4 Analog Stage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353
13.2.5 Power Stage and Electric Protections . . . . . . . . . . . . . . . . 356
13.3 Software Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 356
Reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 360
14 The Geomagic Touch Haptic Device . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 361
14.1 Characteristics of the Geomagic Touch Manipulator . . . . . . . . . . 361
14.1.1 Kinematics of the Full Five DoF Robot . . . . . . . . . . . . . . 361
14.1.2 Direct Kinematics of the Full Five DoF Robot . . . . . . . . 363
14.1.3 Differential Kinematics of the Full Five DoF Robot . . . . 364
14.1.4 Dynamics of the Full Five DoF Robot . . . . . . . . . . . . . . . 365
14.2 Simplified Three DoF Geomagic Touch . . . . . . . . . . . . . . . . . . . . . . 368
14.2.1 Kinematics of the Three DoF Robot . . . . . . . . . . . . . . . . . 368
14.2.2 Direct Kinematics of the Three DoF Robot . . . . . . . . . . . 369
14.2.3 Differential Kinematics of the Three DoF Robot . . . . . . . 370
14.2.4 Dynamics of the Three DoF Robot . . . . . . . . . . . . . . . . . . 371
14.2.5 Linear Parametrization of the Three DoF Robot . . . . . . . 372
Part I
Preliminaries
Chapter 1
A General Overview of Robot
Manipulators
Abstract Robot manipulators are a very particular kind of robots. They are mainly
employed in the industry and therefore they are also known as industrial robots.
Although they can be used for very specialized tasks, their main characteristic is the
reprogrammability, i.e. the possibility of using the same manipulator for different
tasks just by changing the control program code. Great accuracy and repeatability
are expected and necessary to accomplish the desired jobs, and these depend on
a combination of physical mechanical characteristics as well as on the design of
appropriate control schemes. Due to the high nonlinear nature of robot manipulators,
control design represents on its own quite a challenge.
1.1 Brief History of Robot Manipulators
Robot manipulators were created in the twentieth century, although some kind of
automated mechanical machines existed as early in history as in ancient Greece
and Babylon. It is well known that the term robot comes from the Czech word
robota and means subordinated work. It was first used in the science fiction play
Rossum’s Universal Robots by Karel C̆apek. Outside science fiction, the first robots
were created after the Second World War as a solution to the problem of handling
hazardous materials, with the seminal works of Goertz (published several years
after their development). Those first robots were of the master-slave type with a
mechanical coupling. The objective of these mechanisms was to mimic the arm and
hand movements of the operator. Later, the mechanical coupling was substituted by
electrical and hydraulic actuators. In the same decade, George C. Devol developed
a fully automated mechanism called programmed articulated transfer device. The
main novelty of this device was its capability to follow a set of instructions that
could be reprogrammed. Following this idea, the first industrial robot was presented
in 1959 by Unimation, Inc. The manipulator called Unimate, designed by Joseph F.
Engelberger, was successfully installed in a General Motors assembly line in 1961.
After these first experimental designs, a boom of commercial robotic arms occurred
in the 1960s. The American Machine and Foundry (AMF) company introduced the
programmable cylindrical manipulator VERSATRAN in 1962, designed by Harry
© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 3
M. A. Arteaga et al., Local Stability and Ultimate Boundedness in the Control
of Robot Manipulators, Lecture Notes in Electrical Engineering 798,
https://doi.org/10.1007/978-3-030-85980-0_1
4 1 A General Overview of Robot Manipulators
Johnson and Veljko Milenkovic. In the same decade, the Freddy arm, which was
pioneer in integrating the manipulator itself with vision and intelligent systems,
was developed at the University of Edinburgh. In 1969, Victor Scheinman invented
the Stanford arm, which was the first six-axes manipulator with a spherical wrist,
following the suggestions for easier arm solutions proposed by Donald L. Pieper in his
PhD thesis. Years later, Scheinman contributed to the design of the also famous MIT
and PUMA arms. In the next two decades, computer-controller robotic manipulators
were widely adopted in the industry, at first mainly in the automotive, but latter
became gradually employed also in different areas such as chemical, electronics,
and metal industries.
The theory of robotic manipulation was developed at the same time as its phys-
ical counterpart. In 1955, Jacques Denavit and Richard S. Hartenberg proposed a
systematic method to analyze the kinematics of a general open-chain mechanisms.
By employing matrix algebra, they were able to represent the position and orienta-
tion of any two coordinate frames as an homogeneous transformation matrix. These
coordinate frames can be in turn attached to any link of the manipulator, thus solving
the direct kinematics problem. Inverse kinematics turned out to be more involved,
even for open-chains, and several methods were developped to obtain a closed or an
approximate solution. As mentioned above, in 1968 Donald L. Pieper proposed a
design method for robotic manipulators called 321 kinematic structure, which always
leads to a closed form solution for the inverse kinematics problem. This kind of struc-
ture is still used in most of the commercially available robotic arms. The study of the
dynamics and its application to the nonlinear control problem was a very intensive
research topic in the 1980s and early 1990s, resulting in most of the well known
nonlinear control techniques for industrial robots such as sliding modes, adaptive,
and passivity-based controllers.
With the millennium change, a new perspective in robotic manipulators arose
as well. While in the 20th century the great majority of the installed robots were
intended for industrial purposes, in the first two decades of the 21th century an
increasing number of applications are focused on human-centered systems and con-
sequently on safe human-robot interaction. One example of such devices is the da
Vinci Surgical System, developed by Intuitive Surgery Inc. Approved for its usage in
human interventions in 2000, nowadays around 5000 units are installed worldwide.
Some other examples of robots outside industry are the ones employed in hazardous
environments (field robotics), those used to enhance human capabilities (human aug-
mentation) and those designed to improve the quality of life (service robotics). All
these trends have in common that the manipulators are no longer located in highly
structured environments. Such unstructured nature represents a great challenge for
the research community in a number of different areas, which in turn makes multi-
disciplinary and collaborative research mandatory for the upcoming years.
1.2 Industrial Robots 5
Fig. 1.1 Robots A255 and A465 by CRS Robotics and the OMNI Phantom at the Laboratory for
Robotics of the National Autonomous University of Mexico
1.2 Industrial Robots
Robot manipulators are also known as industrial robots. Figure 1.1 shows those avail-
able at the Laboratory for Robotics of the School of Engineering of the National
Autonomous University of Mexico.
First of all, it is necessary to define what a robot manipulator is. The International
Organization for Standardization provides Definition 1 of manipulating industrial
robots in ISO 8373.
Definition 1 An industrial robot is an automatically controlled, reprogrammable,

multipurpose, manipulator programmable in three or more axes, which may be either
fixed in place or mobile for use in industrial automation applications.
One of the key elements of the previous definition is the reprogrammability since
it implies that the very same manipulator can be used for many possible applications,
and that with good accuracy and flexibility. To understand how this can be done, it
is better to begin with the most basic elements of a robot manipulator:
1. Links (connected by joints)
2. Joints:
a. Revolute (R)
b. Prismatic (P)
A revolute joint allows to rotate two links around the joint axis, while the prismatic
joint allows a linear displacement of the links as depicted in Fig. 1.2.
The joint variables are usually denoted by θi for revolute joints and by di for the
prismatic ones, where i = 1, . . . , n. n is the total number of joints of the manipulator
and it is known as the number of Degrees of Freedom (DOF) of the manipulator. A
Fig. 1.2 Revolute and Revolute Prismatic

prismatic joints connecting
two robot links
2D
3D
robot should have 6 DOF because 3 of them are necessary for arbitrary positioning
and 3 more for arbitrary orientation. A smaller number of DOF prevents achieving
any arbitrary position and orientation of the robot’s end-effector, while a bigger
number makes the manipulator to be kinematically redundant. Roughly speaking,
the manipulator’s reachable workspace is the total volume spanned by the robot’s
end-effector when it executes all possible movements. On its own this definition
may just be of little utility, except for avoiding collisions. More important is the
dexterous workspace, within which the manipulador can reach any position with
arbitrary orientation. But reaching a position is kind of fuzzy. The precision of a
manipulator is a measure of how close the robot’s end-effector can reach a point
within its workspace. The repeatability on the other hand is a measure of how close
it can reach a point taught with anteriority. Usually, digital encoders are employed to
measure the value of a joint variable, while the end-effector position and orientation
are computed by taking into consideration the kinematic structure of the manipulator.
For that reason, industrial robots are designed to be rigid.
1.3 Common Kinematic Arrangements
As it will be discussed later in full detail in Chap. 2, the inverse kinematics problem
can be splitted in two easier challenges, inverse position and inverse orientation. To
achieve this, the last three joints of industrial robots are chosen to have the structure
of a spherical wrist, while for the first three joints there are usually five categories:
articulated, spherical, SCARA, cylindrical and Cartesian manipulators.
1.3 Common Kinematic Arrangements 7
1.3.1 Articulated Manipulator
This arrangement is also known as revolute or anthropomorphic. The three robots

in Fig. 1.1 are of this kind, while Fig. 1.3 shows that this arrangement is made up of
three revolute joints (RRR) and depicts the so-called elbow manipulator, where both
axes z 1 and z 2 are parallel and perpendicular to z 0 .
The workspace of an articulated manipulator is shown in Fig. 1.4.
1.3.2 Spherical Manipulator
If in the previous configuration the third joint is replaced by a prismatic one (RRP),
then the spherical manipulator is gotten as shown in Fig. 1.5.
The name of this configuration comes from the fact that spherical coordinates can
straightforwardly be employed to define the position of the end-effector or the wrist
center with respect to a frame fixed at the intersection of the z 1 and z 2 axes. The
corresponding workspace can be seen in Fig. 1.6.
Fig. 1.3 Articulated elbow z0

manipulator
z1 z2
θ2 θ3
shoulder
elbow
θ1
body
base
θ3
θ2
θ1
side view top view
Fig. 1.4 Workspace of an articulated elbow manipulator
Fig. 1.5 Spherical z0

manipulator
z1
θ2
z2
θ1 d3
1.3.3 SCARA Manipulator
The Selective Compliant Articulated Robot for Assembly (SCARA) manipulator does
also have a RRP arrangement, but with a different design (the joint axes z 0 , z 1 and
z 2 are parallel) as shown in Fig. 1.7.
This configuration is widely used in assembly tasks and it is different in appearance
to the spherical manipulator despite having both a RRP structure. The corresponding
workspace can be seen in Fig. 1.8.
Fig. 1.6 Workspace of a

spherical manipulator
Fig. 1.7 SCARA z0 z1 z2

manipulator
θ2
θ1
d3
1.3.4 Cylindrical Manipulator
The cylindrical manipulator has a RPP arrangement, as shown in Fig. 1.9. Since the
first joint is revolute and the last two are prismatic, this configuration owns its name
due to the fact that cylindrical coordinates can be employed to describe the end-
effector position with respect to the base. The corresponding workspace is shown in
Fig. 1.10.
Fig. 1.8 Workspace of a

SCARA manipulator
Fig. 1.9 Cylindrical d3

manipulator
z2
z1
d2
z0
θ1
Fig. 1.10 Workspace of the

cylindrical manipulator
1.3.5 Cartesian Manipulator
The Cartesian manipulator has all of its joints prismatic (PPP), as it is shown in
Fig. 1.11. For this kind of manipulators, the end-effector position, or the center of the
wrist, are directly the Cartesian coordinates given by the joint variables leading to the
simplest of all configurations. The corresponding workspace is shown in Fig. 1.12.
Fig. 1.11 Cartesian d2

manipulator
z1 d3
z0
z2
d1
Fig. 1.12 Workspace for a

Cartesian manipulator
1.4 Wrists and End-Effectors
1.4.1 Spherical Wrist
The connection between the arm and the end-effector is called the wrist. In the
previous section the spherical wrist was mentioned many times because, while the
manipulator’s main body can take any of the five basic configurations shown before,
the last three joints are usually chosen to form precisely a spherical wrist, where its
three axes intersect at one single point known as its center. This is shown in Fig. 1.13.
The particular arrangement of the spherical wrist largely simplifies the inverse
kinematic analysis as explained in detail in Sect. 2.3. It is worthy pointing out that
the wrist may just have less than three joints for some manipulators, which diminishes
the capability of reaching any arbitrary orientation.
Fig. 1.13 Spherical wrist
pitch
roll
yaw
1.4 Wrists and End-Effectors 13
Fig. 1.14 Gripper as

end-effector of an industrial
robot
1.4.2 Common End-Effectors
The main tool of the manipulator is located at the end of the wrist and it is called
end-effector. Perhaps the most usual one is a simple gripper as shown in Fig. 1.14.
Since the gripper can just open and close, a more sophisticated end-effector can
be a mechanical hand, for example. Some other important tools are soldering irons,
spray guns, cameras and some times even just a single finger (see Fig. 1.1).
1.5 Some Other Important Issues to Take into Account
Other important aspects to consider when working with robot manipulators are the
following:
1. To move each of the joints some actuators are needed, being the most commonly
used:
a. Electric actuators, as DC, AC or induction motors can be employed in robot
manipulators. They are usually cheap, clean and silence, which explains their
popularity. Also, DC motors dynamics can be straightforwardly included in
the general robot model, thus allowing an easier design and implementation
of control laws.
b. Hydraulic actuators are fast and they can generate big torques, which allows
the manipulator to move heavy loads. However, they require much more
peripheral equipment like pumps, there may be some hydraulic fluid leaks
and they are noisy.
c. Pneumatic actuators are cheap, but they are difficult to control, so that its
applicability in robotics is not too much.
2. Application area: Depending on the tasks, the robot manipulators can be classi-
fied as assembly or non-assembly. The first class corresponds usually to small
electrically driven robots, while to the second one belong those manipulators
capable of moving huge heavy loads or those employed for handling different
items.
3. Control approach: The precision that a robot manipulator can reach depends
both on physical mechanical issues and on the control technique employed for
the actuators. Choosing a control scheme depends on many factors, but specially
on the task to be accomplished and the information available to implement the
control law. The easiest, yet not meaningless, task is position regulation, while
some of the most challenging ones is bilateral teleoperation with time varying
delays. Other needs can be the reconstruction of non available measurements or
model parameter estimation. This books deals with all these topics.
References
Fu KS, Gonzalez RC, Lee CSG (1987) Robotics: control, sensing, vision, and intelligence. McGraw-
Hill, USA
Goertz RC, Thompson WM (1954) Electronically controlled manipulator. Nucleonics (US) Ceased
publication 12
Goertz RC (1954) Mechanical master-slave manipulator. Nucleonics (US) Ceased publication 12
Murray RM, Li Z, Sastry SS (1994) A mathematical introduction to robotic manipulation. CRC
Press, Boca Raton, FL
Ortega R, Spong MW (1989) Adaptive motion control of rigid robots: a tutorial. Automatica
25(6):877–888
Sciavicco L, Siciliano B (2000) Modeling and control of robot manipulators, 2nd edn. Springer,
London
Siciliano B, Khatib O (2016) Springer handbook of robotics. Springer
Slotine JJE, Li W (1987) On the adaptive control of robot manipulators. Int J Robot Res 6(3):49–59
Slotine J-JE (1985) The robust control of robot manipulators. Int J Robot Res 4(2):49–64
Spong MW, Hutchinson S, Vidyasagar M (2006) Robot modeling and control. Wiley, USA
Chapter 2
Position, Orientation and Velocity
of Rigid Robot Manipulators
Abstract In order to assign a task to a robot manipulator, it is a key matter to describe

the position, orientation and velocity of its tool with respect to an inertial frame,
usually located at the base of the robot. Although the end result may be involved, the
procedure to obtain such a description is rather direct just by defining as many extra
coordinate frames as degrees of freedom the manipulator has and by describing the
relationship between couples of coordinate systems until arriving to the one fixed at
the robot end-effector, where the working tool should be. This allows to define the
orientation and position of the end-effector as a function of joint coordinates. Also
in a systematic fashion it is possible to compute the linear and angular velocities of
the end-effector as a function of both joints positions and velocities.
2.1 Rigid Motions and Homogeneous Transformations
Regulating the position and orientation of a rigid robot manipulator may be quite
a big challenge, beginning with how to describe both quantities in the first place.
A common solution consists in establishing as many coordinate frames as degrees
of freedom the robot has, plus one for the end-effector, where the operating tool is
assumed to be fixed. This allows to compute in a rather direct way the relationship
between two coordinate systems and then just to accumulate the effect of all frames.
2.1.1 Rotations
Consider Fig. 2.1, where a point p ∈ R3 is represented in the coordinate frame ox0 ,y0 ,z0
as a vector. This is a very common representation in Cartesian coordinates where
each axis, x0 , y0 and z 0 , has associated a unit vector in its own direction, i0 , j 0 and
k0 respectively. Therefore, the vector p can be obtained as the following sum:
p = px0 i0 + p y0 j 0 + pz0 k0 . (2.1)

https://doi.org/10.1007/978-3-030-85980-0_2
16 2 Position, Orientation and Velocity of Rigid Robot Manipulators
Fig. 2.1 Representation of a z0

point p in the coordinate
frame ox0 ,y0 ,z 0
k0
j0
y0
i0 O
x0
Since the coordinate system ox0 ,y0 ,z0 is right handed, the vectors i0 , j 0 and k0
satisfy
k0 = i0 × j 0 j 0 = k0 × i0 and i0 = j 0 × k0 , (2.2)
and
i0 · i0 = 1 j0 · j 0 = 1 and k0 · k0 = 1. (2.3)
The last relationships imply of course that i0 , j 0 and k0 are unit vectors. Also, the dot
product of different vectors is always zero, i.e.
i0 · j 0 = 0 j0 · k0 = 0 and k0 · i0 = 0. (2.4)
Recall that the dot product is commutative.

Instead of using (2.1), a more practical notation is the following
⎡ ⎤
px0
p = ⎣ p y0 ⎦ . (2.5)
pz0
Consider now Fig. 2.2, where the very same vector is depicted, but this time together
with another coordinate frame ox1 ,y1 ,z1 .
Clearly, p can be expressed as
p = px1 i1 + p y1 j 1 + pz1 k1 , (2.6)

2.1 Rigid Motions and Homogeneous Transformations 17
Fig. 2.2 Representation of a z0

point p in the coordinate x1
frame ox1 ,y1 ,z 1
p
z1
i1 k1
y0
O
j1
x0
y1
or
⎡ ⎤
px1
p = ⎣ p y1 ⎦ . (2.7)
pz1
While (2.1) and (2.6) are still perfectly compatible, (2.5) and (2.7) make little sense,
and the reason may be rather obvious. The first couple of equations provide a sum of
vectors, while the second one provide how those vectors are expressed in different
coordinate frames. Therefore, it would be more convenient to rewrite (2.1) and (2.6)
as
⎡ ⎤ ⎡ ⎤
px0 px1
0
p = ⎣ p y0 ⎦ and 1 p = ⎣ p y1 ⎦ , (2.8)
pz0 pz1
respectively, where in general 0 p = 1 p despite they describe the very same vector p.
The reason for defining two different coordinate frames is that this might constitute
an advantage for the representation of a particular point or vector in the space, but
now it is necessary to find the relationship between 0 p and 1 p. In fact, from (2.1)
and (2.6) one gets
p = px0 i0 + p y0 j 0 + pz0 k0 = px1 i1 + p y1 j 1 + pz1 k1 . (2.9)
This relationship can better be understood when both coordinates systems are
removed as shown in Fig. 2.3.
Fig. 2.3 Representing p

purely as sum of vectors
p
px1 i1
pz0 k0
px0 i0 + py0 j 0
py1 j 1 + pz1 k1
By using the properties given in (2.3) and (2.4), one can easily get
px0 = p · i0 = px1 i1 · i0 + p y1 j 1 · i0 + pz1 k1 · i0 . (2.10)
In the very same fashion it is possible to obtain
p y0 = px1 i1 · j 0 + p y1 j 1 · j0 + pz1 k1 · j 0 (2.11)

pz0 = px1 i1 · k0 + p y1 j 1 · k0 + pz1 k1 · k0 . (2.12)
The last relationships can be written in compact form

0
p = 0 R1 1 p, (2.13)
where
⎡ ⎤
i1 · i0 j 1 · i0 k1 · i0
0
R1 = ⎣ i1 · j 0 j 1 · j 0 k1 · j 0 ⎦ . (2.14)
i1 · k0 j 1 · k0 k1 · k0
0
R1 ∈ R3×3 represents a transformation which maps the vector p expressed in ox1 ,y1 ,z1 ,
i.e. 1 p, to its representation in the frame ox0 ,y0 ,z0 , i.e. 0 p. Note that the very same
procedure can be employed to find the transformation from ox0 ,y0 ,z0 to ox1 ,y1 ,z1 :
1
p = 1 R0 0 p, (2.15)
where
⎡ ⎤
i0 · i1 j 0 · i1 k0 · i1
1
R0 = ⎣ i 0 · j 1 j 0 · j 1 k 0 · j 1 ⎦ . (2.16)
i0 · k1 j 0 · k1 k0 · k1
Quite obviously, 1 R0 is the inverse transformation of 0 R1 . By taking into account

that the dot product is commutative, one can conclude that
1
R0 = 0 R−1
1 = R1 .
0 T
(2.17)
A matrix whose inverse is its own transpose is said to be orthogonal and this implies
that the columns of 0 R1 must be unit vectors orthogonal to each other. Furthermore, in
general the determinant of an orthogonal matrix takes the only two possible values
±1 since the determinant of a matrix equals that of its transpose. Now, consider
writing the columns of 0 R1 as r1 , r2 , r3 ∈ R3 . The meaning of these columns is easy
to understand. Just choose 1 p = i1 = [1 0 0]T , which means that r1 describes the
direction of the x1 -axis but in ox0 ,y0 ,z0 . The same can be said for r2 and r3 respectively
for the y1 and z 1 axes. But since 0 R1 does not change neither the orientation nor the
magnitude of a vector but just the coordinate frame where the vector is expressed,
then r1 , r2 and r3 must own the very same properties as i1 , j 1 and k1 . On the other
hand, it is possible to compute the determinant of 0 R1 as:
0
det R1 = rT1 (r2 × r3 ) . (2.18)
Since ox1 ,y1 ,z1 is aright handed coordinate system, then one must have r1 = r2 × r3 .
Therefore it is det 0 R1 = rT1 r1 = 1. This means that this particular set of orthogonal
matrices have only +1 as determinant and they are called rotation matrices and they
are said to belong to the group S O(3), standing for Special Orthogonal Group of
dimension 3.
Example
Understanding the meaning of the columns of 0 R1 actually allows to compute it in a

more simple way than using (2.14). Consider a basic rotation, i.e. a rotation around
a main axis x, y or z. The most common one takes place around the z-axis, as shown
in Fig. 2.4. Note that the right hand rule defines the positive value of θ .
At the beginning one may think that ox0 ,y0 ,z0 and ox1 ,y1 ,z1 completely coincide, so
that any vector would take the very same values in each of them. But when frame
ox1 ,y1 ,z1 is rotated by an angle θ around z 0 so that (x0 , y0 ) and (x1 , y1 ) are no longer
aligned, on the contrary to z 0 and z 1 . Therefore, the vector k1 in ox0 ,y0 ,z0 is still k0 .
Otherwise said it is
⎡ ⎤
0
r3 = k 1 = k 0 = k = ⎣ 0 ⎦ . (2.19)
1
Keep in mind that the regular definition of i, j and k, i.e.

Fig. 2.4 Basic rotation y0

around the z axis
y1 sin(θ)
cos(θ) x1
θ
cos(θ)
sin(θ)
θ
x0
z0 ,z1
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
1 0 0
i = ⎣0⎦ j = ⎣1⎦ and k = ⎣0⎦ (2.20)
0 0 1
is used and recall that r1 and r2 are actually i1 and j 1 but expressed in ox0 ,y0 ,z0 . Trivially
from Fig. 2.4 one gets
⎡ ⎤
cos(θ )
r1 = ⎣ sin(θ ) ⎦ (2.21)
0
and
⎡ ⎤
− sin(θ )
r2 = ⎣ cos(θ ) ⎦ . (2.22)
0
This way, the rotation matrix for this example is given by

⎡ ⎤
cos(θ ) − sin(θ ) 0
0
R1 = r1 r2 r3 = ⎣ sin θ cos(θ ) 0 ⎦ . (2.23)
0 0 1
Finally, since the rotation takes place around a main axis it is customary to use the
notation Rz,θ to designate the corresponding rotation matrix.
Note that when θ = 0◦ then it holds
Rz,0 = I (2.24)
because in the very end no rotation is taking place. On the other hand, if after rotating
around z by an angle θ , a second rotation takes place also around z by a new angle φ,
then one actually has one single basic rotation around z but by a total angle θ + φ.
In fact, it is not difficult to show that the following relationship is satisfied
Rz,θ+φ = Rz,θ Rz,φ . (2.25)
Combining (2.24) with (2.25) yields to the conclusion that
R−1
z,θ = Rz,−θ . (2.26)
This result was expected since it is equivalent to undo the rotation of θ to return
ox1 ,y1 ,z1 to its original position coincident with ox0 ,y0 ,z0 . Finally, the other two basic
rotations can easily be computed as:
⎡ ⎤
1 0 0
Rx,ψ = ⎣ 0 cos(ψ) − sin(ψ) ⎦ (2.27)
0 sin(ψ) cos(ψ)
⎡ ⎤
cos(φ) 0 sin(φ)
R y,φ =⎣ 0 1 0 ⎦. (2.28)
− sin(φ) 0 cos(φ)
Quite obviously Rx,ψ and R y,φ share the same properties as Rz,θ .
2.1.2 Composition of Rotations
Recall that the rotation matrix 0 R1 in (2.13):

0
p = 0 R1 1 p, (2.29)
represents a rotational transformation between the coordinate systems ox0 ,y0 ,z0 and
ox1 ,y1 ,z1 . As seen for basic transformations, rotating twice around the very same axis
results in a new basic transformation with the sum of two angles as total rotation.
However, as shown in (2.25) this is equivalent to multiplying the basic rotations. It
turns out that this fact is valid in general as long as the new rotation is carried out with
respect to the current frame. To check this out, suppose there is a third coordinate
frame ox2 ,y2 ,z2 , so that the vector p has now three representations 0 p, 1 p and 2 p,
depending on the particular coordinate frame. There must exist rotation matrices so
that the following relationships hold

0
p = 0 R1 1 p (2.30)
0
p = 0 R2 2 p (2.31)
1
p = 1 R2 2 p, (2.32)
where i R j ∈ S O(3). Note that 0 R1 and 0 R2 represent rotations with respect to ox0 ,y0 ,z0 ,
while 1 R2 does with respect to ox1 ,y1 ,z1 . Combining (2.32) and (2.30) yields
0
p = 0 R1 1 R2 2 p. (2.33)
Therefore, from (2.31) and (2.33) one concludes that

0
R2 = 0 R1 1 R2 . (2.34)
As a matter of fact, it is easy to show that this is a general rule, i.e. for n + 1 coordinate
systems it holds
0
Rn = 0 R1 · · · n−1 Rn . (2.35)
Note, however, that the key assumption to get (2.35) is that every rotation of the chain
is carried out with respect to the current frame. The question is what happens when-
ever the rotations are carried out with respect to a fixed frame (e.g. the inertial one).
Example
Suppose that a 90◦ rotation is carried around y0 followed by another 90◦ rotation
around z 0 (not around z 1 !). The result can be appreciated in Fig. 2.5.
Just as explained before, rotations matrices can be obtained by inspection. Based
on the figure one has
z0
y2
y0 y1 z2
x0 z1
x1 x2
Fig. 2.5 Rotations with respect a fixed frame

z0 y2
y0 y1 x2
x0 z1 z2
x1
Fig. 2.6 Rotations with respect to current frames
⎡ ⎤
0 01
0
R̄1 = ⎣ 0 1 0 ⎦ , (2.36)
−1 0 0
and
⎡ ⎤
1 0 0
1
R̄2 = ⎣ 0 0 1 ⎦ , (2.37)
0 −1 0
where the notation (¯·) is used to distinguish the rotations with respect to current
frames (although the first one is the very same). This means that
⎡ ⎤⎡ ⎤ ⎡ ⎤
0 01 1 0 0 0 −1 0
0
R̄2 = 0 R̄1 1 R̄2 = ⎣ 0 1 0 ⎦ ⎣ 0 0 1 ⎦ = ⎣ 0 0 1 ⎦ . (2.38)
−1 0 0 0 −1 0 −1 0 0
Also by inspection it can be appreciated that the result is correct. However, this
approach becomes rather impractical and very involved for arbitrary rotation angles
and multiple coordinate frames. It would be for sure more desirable to carry out
the rotations with respect to current frames, but the result is quite different if one
just write down the basic rotation matrices and does the computation, as shown in
Fig. 2.6.
Based on the figure one gets
⎡ ⎤ ⎡ ⎤
0 01 0 −1 0
0
R1 = ⎣ 0 1 0 ⎦ and 1
R2 = ⎣ 1 0 0 ⎦ . (2.39)
−1 0 0 0 0 1
Therefore, the total rotation is

⎡ ⎤⎡ ⎤ ⎡ ⎤
0 01 0 −1 0 001
0
R2 = 0 R1 1 R2 = ⎣ 0 1 0 ⎦ ⎣ 1 0 0 ⎦ = ⎣ 1 0 0 ⎦ . (2.40)
−1 0 0 0 0 1 010
z0 z1
y0 y1 y1
x0 z1 x1
x1
z1
y2
y1
x1 z2
x2
Fig. 2.7 Substituting rotations with respect to a fixed frame by rotations with respect to current
frames
Since 0 R2 in (2.40) is not the same as that in (2.38), a natural question is whether it is
possible to get the same result as in (2.38) but with rotations with respect to current
frames. The answer to this question is yes. Consider Fig. 2.7 where the same end result
as in Fig. 2.5 is gotten but with a set of rotations with respect to the corresponding
current frames.
Based on the figure it is easy to see that 0 R2 can be obtained by
⎡ ⎤⎡ ⎤⎡ ⎤⎡ ⎤ ⎡ ⎤
0 01 0 0 −1 0 −1 0 0 01 0 −1 0
0
R̄2 = ⎣ 0 1 0 ⎦ ⎣ 0 1 0 ⎦ ⎣ 1 0 0 ⎦ ⎣ 0 1 0 ⎦ = ⎣ 0 0 1 ⎦ .(2.41)
−1 0 0 10 0 0 0 1 −1 0 0 −1 0 0
I
The key point is that the first two rotations produce an identity matrix, so that in the
very end 0 R̄2 can be obtained by carrying out rotations with respect to the current
frame but in reverse order, i.e.
0
R̄2 = 1 R2 0 R1 . (2.42)
Despite the example is quite trivial, this result can be shown to be general, no matter
how many rotations are considered. In general, if the actual coordinate frame oxb ,yb ,zb
with respect to the frame oxa ,ya ,za is expressed through the rotation matrix a Rb and one
wants to perform a rotation Rα over another coordinate frame oxc ,yc ,zc , the resulting
rotation a Rd can be computed by the composition
a
Rd = a Rb c RTb Rα c Rb , (2.43)
where the last three terms represent a similarity transformation, which is a well
known result of linear algebra. In the example, a = c = 0, b = 1, 0 R1 = Rx,90◦ and
Rα = Rz0 ,90◦ .
2.1.3 Different Parametrizations for Rotation Matrices
The nine elements of a rotation matrix R ∈ S O(3) are not independent. Both columns
and rows are unit vectors representing the orientation of the axes x, y and z of a right-
handed coordinate system. Therefore, in the end only three parameters are necessary
to characterize R. However, by no means these three parameters are unique and there
exist multiple options.
2.1.3.1 Euler Angles
Since only three parameters are necessary to specify any arbitrary rotation, a natural
way to describe them is by means of the so called Euler Angles (φ, θ, ψ). For this
case, the total rotation is the result of three consecutive rotations with respect to the
current frame. Note that it makes no sense at all to carry out two consecutive rotations
around the same axis, so that the set of possible combinations reduces to 12 (XYX,
XYZ, XZX, XZY, YXY, YXZ, YZX, YZY, ZXY, ZXZ, ZYX, and ZYZ).
Figure 2.8 shows the most employed set of Euler Angles, which consists first of a
basic rotation around the z-axis, followed by another around y and the last one also
around z, resulting in
Fig. 2.8 Euler angles z0 , z0

representation ZYZ z1
φ
y1
ψ θ
θ
y0 , y0
φ
y0
φ
θ ψ
x0
x1
x0
x0
0
R1 = Rz,φ R y,θ Rz,ψ
⎡ ⎤⎡ ⎤⎡ ⎤
cφ −sφ 0 cθ 0 sθ cψ −sψ 0
= ⎣ sφ cφ 0 ⎦ ⎣ 0 1 0 ⎦ ⎣ sψ cψ 0 ⎦
0 0 1 −sθ 0 cθ 0 0 1
⎡ ⎤
cφ cθ cψ − sφ sψ −cφ cθ sψ − sφ cψ cφ sθ
= ⎣ sφ cθ cψ + cφ sψ −sφ cθ sψ + cφ cψ sφ sθ ⎦ . (2.44)
−sθ cψ sθ sψ cθ
A more complex problem is the inverse one, i.e. given a rotation matrix R ∈
S O(3), with components ri j , i, j = 1, 2, 3, compute the angles φ, θ , and ψ, such
that the composition (2.44) results in the given matrix. To do this, consider first the
function atan2 = atan2(y, x), which is the arc tangent of two arguments returning
an angle which corresponds to the given quadrant, i.e. is the angle θ such that
x y
cos θ = and sin θ = . (2.45)
x2 + y2 x2 + y2
With this in mind, from 0 R1 it can be seen that if cθ = r33 = ±1 =⇒ sθ = 0, these

angles can be computed as

θ = atan2 1 − r33
2
, r33 (2.46)
φ = atan2 (r23 , r13 ) (2.47)

ψ = atan2 (r32 , −r31 ) . (2.48)
On the other hand, in the degenerated case when r33 = ±1, there are infinitely many
solutions. In fact, it is easy to see that for r33 = 1 the composition degenerates into
two consecutive rotations about the z axis. Therefore, a solution is
θ =0 (2.49)
ψ + φ = atan2(r21 , r11 ). (2.50)
For the case when r33 = −1, a solution is given by
θ =π (2.51)
ψ − φ = atan2(r21 , −r11 ). (2.52)
As in these two cases the solution involves the sum or the difference of φ and ψ, it
is common to set φ = 0 to obtain a single solution.
2.1.3.2 Yaw–Pitch–Roll Angles
Another common way to specify orientation are the so called Yaw–Pitch–Roll

Angles, which consist of three consecutive rotations around x0 , y0 and z 0 , result-
ing in
0
R1 = Rz,φ R y,θ Rx,ψ
⎡ ⎤⎡ ⎤⎡ ⎤
cφ −sφ 0 cθ 0 sθ 1 0 0
= ⎣ sφ cφ 0 ⎦ ⎣ 0 1 0 ⎦ ⎣ 0 cψ −sψ ⎦
0 0 1 −sθ 0 cθ 0 sψ cψ
⎡ ⎤
cφ cθ −sφ cψ + cφ sθ sψ sφ sψ + cφ sθ cψ
=⎣ sφ cθ cφ cψ + sφ sθ sψ −cφ sψ + sφ sθ cψ ⎦ . (2.53)
−sθ cθ sψ cθ cψ
For this representation, the inverse problem solution can be found if r31 = −sθ =
±1 =⇒ cθ = 0, as

θ = atan2 −r31 , 1 − r31
2
(2.54)
φ = atan2 (r21 , r11 ) (2.55)

ψ = atan2 (r32 , r33 ) . (2.56)
In the case of r31 = 1 a solution is
θ = −π/2 (2.57)
φ + ψ = atan2 (−r12 , r22 ) , (2.58)
whereas, if r31 = −1 a suitable solution is
θ = π/2 (2.59)
φ − ψ = atan2(r23 , r22 ). (2.60)
Once again, a closed solution can be found by setting the value of one of the two
angles a priori, e.g. ψ = 0.
2.1.3.3 Axis/Angle Representation
An arbitrary rotation matrix R ∈ S O(3) can always be represented by one single

rotation around an appropriate unitary axis a ∈ R3 expressed in ox0 ,y0 ,z0 :
Fig. 2.9 Axis/angle z0

representation az
θ
β
a
ay
y0
ax α
x0
⎡ ⎤
ax
a = ⎣ ay ⎦ , (2.61)
az
by an angle θ , as shown in Fig. 2.9.

The properties of the rotations with respect to a fixed frame can be exploited to
compute the total rotation of θ degrees around an arbitrary unit vector. This rotation
matrix can be denoted as Ra,θ . According to Fig. 2.9, Ra,θ can be computed by
carrying out five rotations around the main axes of ox0 ,y0 ,z0 . First rotate a around
z 0 by −α degrees, followed by a rotation by −β degrees around y0 . These two
rotations align a with the z 0 axis, where a regular basic rotation of θ degrees can be
performed. The last step is to undo the first two rotations. The desired rotation matrix
can therefore be computed as
Ra,θ = Rz,α R y,β Rz,θ R y,−β Rz,−α . (2.62)
By taking into account that a is unitary, one can conclude that

ay
sin α =
ax + a 2y
2
ax
cos α =
ax + a 2y
2

sin β = ax2 + a 2y
cos β = az .
The final result is given by

⎡ ⎤
ax2 vθ + cθ ax a y vθ − az sθ ax az vθ + a y sθ
Ra,θ = ⎣ ax a y vθ + az sθ a 2y vθ + cθ a y az vθ − ax sθ ⎦ , (2.63)
ax a y vθ − a y sθ a y az vθ + ax sθ az2 vθ + cθ
where cθ = cos(θ ), sθ = sin(θ ) and vθ = vers(θ ) = 1 − cθ .

On the other hand, given a particular rotation matrix R, the challenge consists in
finding both a and θ such that
R = Ra,θ (2.64)
holds. Let’s denote the elements R as ri j for i, j = 1, 2, 3 as before. Then it is possible

to compute

Tr(R) − 1 r11 + r22 + r33 − 1
θ = cos−1 = cos−1 (2.65)
2 2
and
⎡ ⎤
r32 − r23
1 ⎣ r13 − r31 ⎦ ,
a= (2.66)
2 sin(θ ) r − r
21 12
where Tr(·) denotes the trace operator. Note that the representation is not unique
since the very same result can be gotten by using −a as axis and performing −θ
degrees of rotation, i.e.
Ra,θ = R−a,−θ . (2.67)
Furthermore, if θ = 0◦ then R becomes the identity and any vector a can be considered
as the axis of rotation.
2.1.4 Unit Quaternion
The disadvantages of the axis/angle representation can be compensated by means

of the four parameters representation known as the unit quaternion, denoted as Q =
{η, }, where

θ
η = cos (2.68)
2

θ
= sin a, (2.69)
2
where θ and a are the same values as those employed in the axis/angle repre-
sentation in Fig. 2.9. η ∈ R is known as the scalar part of the quaternion, while
T
= x y z ∈ R3 is the vectorial part. These quantities are related by
η2 + 2
x + 2
y + 2
z = η2 + T = 1. (2.70)
Note that unlike the axis/angle representation, a rotation of −θ around −a delivers

the very same quaternion. It is then possible to compute
⎡ 2 ⎤
2 η + x2 −1 2 x y − η z 2 x z + η y
R(η, ) = ⎣ 2 x y + η z 2 η2 + y2 −1 2 y z − η x ⎦ . (2.71)
2 x z −η y 2 y z +η x 2 η2 + z2 − 1
On the contrary, given an arbitrary rotation matrix R ∈ S O(3) with elements ri j for
i, j = 1, 2, 3, it holds
1
η= r11 + r22 + r33 + 1 (2.72)
2⎡ √ ⎤
sign(r32 − r23 )√ r11 − r22 − r33 + 1
1⎣
= sign(r13 − r31 )√−r11 + r22 − r33 + 1 ⎦ , (2.73)
2
sign(r21 − r12 ) −r11 − r22 + r33 + 1
where sign(x) = 1 for x ≥ 0 and sign(x) = −1 for x < 0. Note that in (2.72) and
(2.73) it is assumed that η ≥ 0, meaning that θ ∈ [−π π ].
The unit quaternion gotten using R−1 = RT is denoted by Q−1 and it is given by
Q−1 = {η, −}. (2.74)
Some other useful relationships are
1
η̇ = − T ω (2.75)
2
1
˙ = E(η, )ω, (2.76)
2
where ω is the angular velocity ω = θ̇ a, and
E(η, ) = ηI − S() (2.77)

⎡ ⎤
0 − z y
S() = ⎣ z 0 − x ⎦ . (2.78)
− y x 0
Fig. 2.10 Parallel z1

translation of a coordinate
1
frame p
y1
0
p O1
z0
x1
0
d1
O0 y0
x0
2.1.5 Homogeneous Transformations
Till now it has been shown how a vector can be represented in more than one coor-
dinate frame by means of rotation matrices as long as the origins of all coordinate
frames are coincident. Consider now Fig. 2.10.
Since the coordinate frames are parallel, the unit vectors i0 , j 0 , k0 are necessarily
parallel to i1 , j 1 , k1 , respectively. The vector 0 d 1 ∈ R3 goes from the origin o0
of ox0 ,y0 ,z0 to the origin o1 of ox1 ,y1 ,z1 , expressed in ox0 ,y0 ,z0 . This way, any point p
represented as 0 p and 1 p in the different coordinate frames as before can be related
as a regular sum of vectors, i.e.
0
p = 1p + 0d1. (2.79)
The most general situation arises when the coordinate frame ox1 ,y1 ,z1 is not only
translated but also rotated with respect to ox0 ,y0 ,z0 , as shown in Fig. 2.11. For that case
the relationship (2.79) makes no sense because 1 p is still expressed in ox1 ,y1 ,z1 , while
0
p and 0 d 1 are expressed in ox0 ,y0 ,z0 . However, by comparing Fig. 2.10 with Fig. 2.11
it can be seen that in the very end one has a regular sum of vectors, which makes
only sense if and only if all vectors are expressed in the same coordinate frame. It
turns out that expressing 1 p in ox0 ,y0 ,z0 is trivial if the rotation matrix 0 R1 is known,
so that instead of (2.79) one can employ
0
p = 0 R1 1 p + 0 d 1 . (2.80)
The transformation given by 0 d 1 ∈ R3 and 0 R1 ∈ S O(3) represents a rigid motion.

The set of all rigid motions is known as the Special Euclidean Group S E(3). If an
additional third coordinate frame ox2 ,y2 ,z2 is added so that there is a further rotation
and translation with respect to ox1 ,y1 ,z1 , then the following relationship is also valid
1
p = 1 R2 2 p + 1 d 2 . (2.81)
Fig. 2.11 Sum of vectors for x1

two coordinate frames
1
p
0
p
O1 y1
z0 z1
0
d1
O0 y0
x0
To express 2 p in the base frame, substitute 1 p from (2.81) in (2.80) to get

1
0
p = 0 R1 R2 2 p + 1 d 2 + 0 d 1
= 0 R1 1 R2 2 p + 0 R1 1 d 2 + 0 d 1 . (2.82)
But, on the other hand, since the relationship between 0 p and 2 p is a rigid motion,
then it can be described by
0
p = 0 R2 2 p + 0 d 2 . (2.83)
By comparing (2.82) with (2.83), one directly gets

0
R2 = 0 R1 1 R2 (2.84)
0
d 2 = 0 d 1 + 0 R1 1 d 2 . (2.85)
Equation (2.84) shows that, as before, the total rotation can be obtained by multiplying
0
R1 by 1 R2 , while Eq. (2.85) shows that the position vector from o0 to o2 is the sum
of the vectors from o0 to o1 , i.e. 0 d 1 , and then the vector from o1 to o2 but expressed
in ox0 ,y0 ,z0 , i.e. 0 R1 1 d 2 as it had been deducted before. This can also be clearly
appreciated in Fig. 2.12.
Quite naturally it would be desirable to have only expressions of the form (2.84)
rather than of the form (2.85), i.e. products of matrices resulting in another matrix
of the same kind (a group structure) instead of a sum of them. This goal can be
accomplished by putting all the information of rotation and translation in a single
transformation matrix H ∈ R4×4 of the form

R d
H= T , (2.86)
0 1
known as homogeneous transformation. Since it is assumed that R ∈ S O(3), then it

is rather straightforward to compute:
Fig. 2.12 Sum of vectors for z2

three coordinate frames 2
x2
y2 p
1
p x1
O2
1
d2
0
p O1
y1
z0 z1
0
d1
y0
O0
x0

RT −RT d
H −1 = . (2.87)
0T 1
This way, Eqs. (2.84) and (2.85) can be written in a more compact form as
0
H2 = 0H11H2
0 0 1 1
R1 d 1 R2 d 2
=
0T 1 0T 1
0 1 0 1 0 0
R1 R2 R1 d 2 + 0 d 1 R2 d 2
= = , (2.88)
0T 1 0T 1
T
where 0T = 0 0 0 . However, the relationship between 0 p and 1 p given by
0
p = 0 R1 1 p + 0 d 1 ,
cannot be carried out directly because 0 p, 1 p ∈ R3 . A simple solution consists in

defining the homogeneous representation of these vectors, i.e.
0
p
0
p̄ = , (2.89)
1
and
1
p
1
p̄ = . (2.90)
1
It is also said that 0 p̄ and 1 p̄ are expressed in homogeneous coordinates. It is easy to

see that the transformation (2.80) is equivalent to
0
p̄ = 0 H 1 1 p̄. (2.91)
The set of all 4 × 4 matrices of the form (2.86) is denoted by E(3) (Euclidean group
of order 3). The basic homogeneous transformations are
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
1 0 0 a 1 0 00 1 0 0 0
⎢0 1 0 0⎥ ⎢0 1 0 b⎥ ⎢0 1 0 0⎥
T x,a =⎢
⎣0
⎥ , T y,b = ⎢ ⎥ , T z,c = ⎢ ⎥, (2.92)
0 1 0⎦ ⎣0 0 1 0⎦ ⎣0 0 1 c⎦
0 0 0 1 0 0 01 0 0 0 1
for translations, and

⎡ ⎤ ⎡ ⎤ ⎡ ⎤
1 0 0 0 cφ 0 sφ 0 cθ −sθ 0 0
⎢0 cα −sα 0⎥ ⎢ 0 1 0 0⎥ ⎢ sθ cθ 0 0⎥
Rx,α = ⎢ ⎣0 sα cα
⎥ , R y,φ = ⎢ ⎥ , Rz,θ = ⎢ ⎥ ,(2.93)
0⎦ ⎣−sφ 0 cφ 0⎦ ⎣0 0 1 0⎦
0 0 0 1 0 0 0 1 0 0 0 1
for rotations, regarding the axes x, y and z, respectively. It is customary to set the
notation

nsad
H= , (2.94)
0001
where in view of the definition of H, n, s and a represent the directions of the axes
x1 , y1 and z 1 with respect to ox0 ,y0 ,z0 , while d represents the position of the origin of
o1 expressed in the same coordinate frame. Most important is that the same rules of
composition of rotations matrices are valid for homogeneous transformations, both
with respect to current or fixed frames.
2.1.6 Skew Symmetric Matrices
A square matrix S is said to be skew symmetric if and only if
ST + S = O. (2.95)
The set of all 3 × 3 skew symmetric matrices is denoted by SS(3). If S ∈ SS(3) and
it has si j , i, j = 1, 2, 3 elements, then (2.95) is equivalent to nine equations of the
form
si j + s ji = 0. (2.96)
From this relationship it is obvious that sii = 0, while the rest of the elements of S
must satisfy si j = −s ji (for i = j). This means that S has only three free elements.
T
Let’s say that those three parameters are put together in a vector a = ax a y az to
define a particular skew symmetric matrix of the form
⎡ ⎤
0 −az a y
S(a) = â = (â) = ⎣ az 0 −ax ⎦ . (2.97)
−a y ax 0
S(a) can be seen as a linear operator since given any two vectors a, b ∈ R3 , and any
two scalars α, β ∈ R it holds
S(αa ± βb) = αS(a) ± βS(b). (2.98)
But perhaps more important is that it is always true
S(a)b = a × b, (2.99)
where a × b is the regular vector cross product.

Consider now a R ∈ S O(3) and recall that this kind of matrices can represent
two vectors a and b in other coordinate frames without changing the vectors them-
selves. Therefore, a cross product can be carried out before or after changing the
representation without affecting the end results, i.e. the following holds always
R(a × b) = Ra × Rb. (2.100)
This property can be proved by direct computation, but care should be taken since
this relationship is not true in general but only for orthogonal matrices.
Another interesting property for any R ∈ S O(3) and any a, b ∈ R3 , is that
after (2.99) and (2.100) one has

RS(a)RT b = R a × RT b

= (Ra) × RRT b
= (Ra) × b
= S (Ra) b. (2.101)
Since b is completely arbitrary one must have as well
RS(a)RT = S (Ra) . (2.102)
Now, suppose that the rotation matrix R is a function of the single variable θ , i.e.
R = R(θ ) ∈ S O(3). Since R is orthogonal for all θ it follows
R(θ )RT (θ ) = I. (2.103)

By taking the derivative with respect to θ one obtains
dR T dRT
R (θ ) + R(θ ) = O. (2.104)
dθ dθ
If it is defined
dR T
S= R (θ ), (2.105)
dθ
then one has
T
dR T dRT
ST = R (θ ) = R(θ ) . (2.106)
dθ dθ
Consequently, Eq. (2.104) can be written as
S + ST = O. (2.107)
In other words, the matrix S defined by (2.105) is skew-symmetric. By multiplying

both sides of (2.105) by R and recalling that R is orthogonal, it is
dR
= SR(θ ). (2.108)
dθ
Equation (2.108) is very important, since it says that computing the rotation matrix
derivative equals to multiply the rotation matrix R by a skew-symmetric matrix S. A
common case occurs when R is a basic rotation or a composition of basic rotations.
Example
Consider R = R y,θ , the basic rotation matrix

⎡ ⎤
cos(θ ) 0 sin(θ )
R y,θ =⎣ 0 1 0 ⎦.
− sin(θ ) 0 cos(θ )
By direct computation it is straightforward to show that

⎡ ⎤⎡ ⎤
− sin(θ ) 0 cos(θ ) cos(θ ) 0 − sin(θ )
dR T
S= R (θ ) = ⎣ 0 0 0 ⎦⎣ 0 1 0 ⎦
dθ − cos(θ ) 0 − sin(θ ) sin(θ ) 0 cos(θ )
⎡ ⎤
0 01
= ⎣ 0 0 0 ⎦ = S(j). (2.109)
−1 0 0
Therefore
dR y,θ
= S(j)R y,θ . (2.110)
dθ
In a similar manner, it can be shown that
dRx,θ dRz,θ
= S(i)Rx,θ , and = S(k)Rz,θ . (2.111)
dθ dθ
Example
Let Ra,θ be a rotation matrix of an angle θ over the axis vector a given in (2.66):
⎡ ⎤
r32 − r23
1 ⎣ r13 − r31 ⎦ .
a=
2 sin(θ ) r − r
21 12
It can be shown that

dRa,θ
= S(a)Ra,θ . (2.112)
dθ
2.1.7 Angular Velocity and Acceleration
In the last section, the relationship between the position and orientation of several
coordinate systems was established by introducing homogeneous transformations. In
this section, it will be discussed how to represent angular velocities and accelerations
in the same context. Suppose that a rotation matrix R is time-dependent, so R =
R(t) ∈ S O(3) for all t ∈ R. By a similar argument as the employed to compute the
derivative of R with respect to θ , it can be computed
Ṙ(t) = S(t)R(t), (2.113)

where S(t) is a skew-symmetric matrix. Moreover, S(t) can be represented as S(ω(t)),

for a unique vector ω(t). Such vector ω(t) is known as the angular velocity of the
rotated frame with respect to a frame fixed in t.
Example
Suppose that R(t) = R y,θ(t) . Then Ṙ(t) = dR(t)

dt
can be gotten by using the chain rule
as
dR dθ
Ṙ(t) = = θ̇ S(j)R(t) = S(ω(t))R(t), (2.114)
dθ dt
where ω = θ̇ j is the angular velocity over the y axis.

Consider a position vector 1 p of a point fixed in the frame ox1 ,y1 ,z1 , which rotates
with respect to the fixed frame ox0 ,y0 ,z0 . The coordinates of 1 p with respect ox0 ,y0 ,z0
are given by
0
p = R(t)1 p. (2.115)
The corresponding linear velocity 0 ṗ is given by

0
ṗ = S(ω)R(t)1 p = S(ω)0 p = ω × 0 p, (2.116)
which is the well known expression of the linear velocity as the cross product of two
vectors. Now, suppose a more general motion of the frame ox1 ,y1 ,z1 with respect to
the frame ox0 ,y0 ,z0 represented by the time-dependent homogeneous transformation
⎡0 ⎤
R1 (t) 0 d 1 (t)
0
H 1 (t) = ⎣ ⎦. (2.117)
0T 1
For the sake of simplicity, time dependency and the indexes 0, 1 will be omitted, i.e.
0
p = R1 p + d. (2.118)
By taking the time derivative of the previous relation and taking into account that 1 p
is constant
0
ṗ = Ṙ1 p + ḋ
= S(ω)R1 p + ḋ
= ω × r + v, (2.119)
where r = R1 p is the vector from the origin o1 to p, expressed in the frame ox0 ,y0 ,z0 ,
and v is the velocity of the origin o1 with respect to ox0 ,y0 ,z0 . If the vector 1 p is not
constant with respect to the frame ox1 ,y1 ,z1 , then one must add the term R1 ṗ, which
is the velocity of 1 p expressed in the frame ox0 ,y0 ,z0 .
An expression to compute the relative acceleration of two coordinate frames can
be found as follows. First, notice that:
d da db
a×b= ×b+a× . (2.120)
dt dt dt
and rewrite (2.119) as
0
ṗ − ḋ = Ṙ1 p + R1 ṗ = ω × (R1 p) + R1 ṗ. (2.121)
To compute the second derivative consider that Ṙ = S(ω)R to get

0
p̈ − d̈ = ω̇ × ( R1 p ) + ω × ( Ṙ1 p ) + ω × (R1 ṗ) + Ṙ1 ṗ + R1 p̈
r ω×r
Ṙ
= ω̇ × r + ω × (ω × r) + ω × (R ṗ) + (ω × R)1 ṗ + R1 p̈
1
= ω̇ × r + ω × (ω × r) + 2ω × (R1 ṗ) + R1 p̈. (2.122)
Therefore, it is obtained
0
p̈ = ω̇ × r + ω × (ω × r) + 2ω × (R1 ṗ) + a, (2.123)
where the term ω̇ × r is known as the transverse acceleration and ω × (ω × r) is

known as the centripetal acceleration of the particle, which points always to the
rotation axis and is orthogonal to it. The term 2ω × (R1 ṗ) represents the Coriolis
acceleration, while a = R1 p̈ + d̈ is the linear acceleration.
Very often, it is necessary to compute the angular velocity due to the rotation
of several frames. In what follows, the expressions of the composition of angular
velocities of two frames ox1 ,y1 ,z1 and ox2 ,y2 ,z2 with respect to a fixed frame ox0 ,y0 ,z0
will be computed.
Given a point p represented in different frames as 0 p, 1 p, and 2 p, the following
relations are satisfied:
0
p = 0 R1 1 p + 0 d 1 (2.124)
1
p = R2 p + d 2
1 2 1
(2.125)
0
p = 0 R2 2 p + 0 d 2 , (2.126)
where
0
R2 = 0 R1 1 R2 , (2.127)
and
0
d 2 = 0 d 1 + 0 R1 1 d 2 . (2.128)
As seen before, all the previous relations are time-dependent functions. On the one
hand, by taking the time derivative of both sides of (2.127), one obtains
0
Ṙ2 = 0 Ṙ1 1 R2 + 0 R1 1 Ṙ2 . (2.129)
On the other hand, 0 Ṙ2 must satisfy

0
Ṙ2 = S(0 ω2 )0 R2 . (2.130)
Now, while the first term in the right hand side of (2.129) is given by
0
Ṙ1 1 R2 = S(0 ω1 )0 R1 1 R2 = S(0 ω1 )0 R2 , (2.131)
the second term in the right hand side of (2.129) must be analyzed more thoroughly.
By employing (2.102)
RS(a)RT = S (Ra) ,
the term 0 R1 1 Ṙ2 can be written as

0
R1 1 Ṙ2 = 0 R1 S(1 ω2 )1 R2
= 0 R1 S(1 ω2 )0 RT1 0 R1 1 R2
= S(0 R1 1 ω2 )0 R2 . (2.132)
By combining the above expressions it is obtained
S(0 ω2 )0 R2 = S(0 ω1 )0 R2 + S(0 R1 1 ω2 )0 R2 . (2.133)
Due to the linearity of the operator S(·), it is concluded that

0
ω2 = 0 ω1 + 0 R1 1 ω2 . (2.134)
In other words, angular velocities can be summed as long as they are expressed in the
same coordinate frame, in this case ox0 ,y0 ,z0 . Furthermore, the above representation
can be extended to any number of frames, e.g. if
0
Rn = 0 R1 1 R2 · · · n−1 Rn , (2.135)
then
0
Ṙn = S(0 ωn )0 Rn , (2.136)
where
0
ωn = 0 ω1 + 0 R1 1 ω2 + 0 R2 2 ω3 + · · · + 0 Rn−1 n−1 ωn . (2.137)
2.2 Direct Kinematics
In this section, the direct kinematics for rigid robots will be developed. The direct
kinematics problem can be formulated as follows: given the values for the joint
variables, compute the position and the orientation of the end-effector. The joint
variables are the angles for the case of revolute joints and the linear displacements
for the case of prismatic joints.
2.2.1 Kinematic Chains
To analyze the kinematics, it could be considered that a robot is composed of rigid

elements connected by joints. Under the assumption that each joint has one degree
of freedom, the effect of each of them can be described by a single real scalar: the
rotation angle for revolute joints and the linear displacement for prismatic joints. The
objective of the direct kinematics is to determine the total effect of all the manipulator
joints in the position and orientation of the end-effector. But first, some conventions
must be adopted to make this analysis in a more systematic way.
Suppose that a robot is composed of n + 1 rigid elements, numbered from 0 to
n, starting at the robot’s base, which is considered to be the 0 rigid element, usually
fixed at the floor or at a table. Suppose it has n joints, numbered from 1 to n. The ith
joint is at the point where the elements i − 1 and i are connected. The corresponding
ith joint variable is denoted as qi , which can be an angle or a displacement, depending
on the type of joint. Fix a coordinate frame to each rigid element, in particular fix the
inertial system ox0 ,y0 ,z0 at the robot’s base. Therefore, the n + 1 frames are defined
in such a way that the ith frame is fixed to the ith element. This means that for every
motion of the robot, the coordinates of any point of the ith element are constant with
respect to the ith frame. This is shown in Fig. 2.13.
Now, suppose that i−1 Ai is the homogeneous transformation matrix which drives
the coordinates of a point on the ith frame to the (i − 1)th frame. The matrix i−1 Ai
is not constant, but it is a function of one single variable qi , i.e.
y3
x3
link 3
y1
d2 x1
z3
link 2 z1 y3
joint 2 z2
link 1 z0 joint 3
x2
joint 1 θ1 θ3
y0
x0 base
(link 0)
Fig. 2.13 Definition of frame coordinates on the different elements of a robot manipulator
i−1
Ai = i−1 Ai (qi ). (2.138)
By convention, the homogeneous matrix which transforms the coordinates of a point

on the jth frame to the ith frame is called transformation matrix and it is commonly
denoted by i T j , which after the discussion of Sect. 2.1.5 is given by
i
T j = i Ai+1 i+1 Ai+2 i+2 Ai+3 · · · j−2 A j−1 j−1 A j if i < j
i
Tj = I if i = j
j −1
i
T j = Ti if i > j
From the above definitions, it follows that the position of any point of the end-
effector is constant when expressed in frame n as mentioned before, independently of
the robot configuration. Denote the position and orientation of the end-effector with
respect to the base system as a vector 0 d n ∈ R3 and a rotation matrix 0 Rn ∈ S O(3),
respectively, and define the homogeneous matrix
0
Rn 0 d n
0
Tn = , (2.139)
0T 1
so that the position and orientation of the end-effector with respect to the base frame
are given by
0
T n = 0 A1 (q1 ) · · · n−1 An (qn ). (2.140)
2.2 Direct Kinematics 43
Each homogeneous transformation i−1 Ai (qi ) has the form

i−1 i−1

Ri di
i−1
Ai = , (2.141)
0T 1
from which one has

i
Rj id j
i
T j = Ai+1
i i+1
Ai+2 · · · j−1
Aj = . (2.142)
0T 1
The rotation matrix i R j expresses the orientation of the jth frame with respect to the
ith frame and it is given by
i
R j = i Ri+1 · · · j−1 R j . (2.143)
The vector i d j can be computed recursively by

i
d j = i d j−1 + i R j−1 j−1 d j . (2.144)
2.2.2 The Denavit-Hartenberg Representation
2.2.2.1 Denavit-Hartenberg Algorithm
Although the n + 1 coordinate frames could in principle be chosen completely arbi-

trarily and the different homogeneous matrices could be computed just by inspection,
it appears to be more useful to have some convention to choose all of them if possible
by following simple rules. This matter is discussed in this section. In fact, perhaps
the most widely employed convention in robotics is the one proposed by Denavit
and Hartenberg (D-H). So as to simplify the discussion, consider first Algorithm 2.1
to choose the location of the n + 1 coordinate frames attached to the robot links.
Algorithm 2.1 Given a robot manipulator with n joints, the following algorithm
assigns the coordinate frames such that the Denavit-Hartenberg convention is satis-
fied:
1. Locate and enumerate the joint axes z0 , . . . , zn−1 . Align the zi axis with the axis
of motion, rotational or linear, of the (i + 1)th joint.
2. Assign an orthonormal right-handed frame ox0 ,y0 ,z0 to the robot base, with the
z0 axis along the axis of motion of joint 1. Locate the origin o0 in a convenient
point over z0 . The axes x0 and y0 can then be located conveniently as long as they
remain orthogonal to the z0 axis and form a right-handed coordinate frame.
3. Locate the origin oi , for i = 1, . . . , n − 1, on the intersection of the zi axis and
the common normal to zi and zi−1 . If zi intersects zi−1 , locate the origin oi on
the intersection. If zi and zi−1 are parallel, locate the origin oi on any convenient
point over zi .
4. Set the axis xi , for i = 1, . . . , n − 1, along the common normal to zi and zi−1
passing through the origin oi , or along the normal direction to the plane zi –zi−1
if zi intersects zi−1 .
5. Set the axis yi , for i = 1, . . . , n − 1, to form a right-handed coordinate frame.
6. Establish the end-effector coordinate frame oxn ,yn ,zn . Locate the origin on in the
most important point of the end-effector, e.g. center of a gripper, tip of the tool,
etc. Set the axis zn parallel to zn−1 passing through on . Set yn conveniently, e.g.
on the direction in which the gripper closes. Finally, set xn to form a right-handed
coordinate frame.
Algorithm 2.1 provides an easy way to locate the different coordinate frames nec-
essary to compute the complete direct kinematics of a robot manipulator. It remains
but to obtain the corresponding homogeneous transformation matrices for each cou-
ple of links i − 1 and i, for i = 1, . . . , n. While this could be done by inspection as
mentioned before, it turns out that the particular frames locations allow to describe
each transformation as the product of four basic transformations. This can better be
understood by analyzing Fig. 2.14. The two coordinate frames are not really inde-
pendent since the xi axis is perpendicular both to zi−1 and zi . This does not mean that
xi−1 and xi point in the same direction but they differ by an angle θi which represents
a rotation around zi−1 . Once this rotation is carried out and xi−1 and xi point in the
same direction, then to align them a translation by di along zi−1 has to be performed,
while to make the origins coincide a new translation by ai along xi has to be done.
Finally, to align zi−1 and zi , a new rotation around xi by an angle αi has to be carried
out. This leads to the following homogeneous transformation:
αi
joint i
zi
Oi
xi
ai
di link i
zi−1
joint i + 1
yi−1
1
link i −
Oi−1
xi−1 θi
Fig. 2.14 The four parameters of the Denavit-Hartenberg convention

i−1
Ai = Rz,θi T z,di T x,ai Rx,αi
⎡ ⎤⎡ ⎤⎡ ⎤⎡ ⎤
cθi −sθi 0 0 100 0 1 0 0 ai 1 0 0 0
⎢ sθi cθi 0 0 ⎥ ⎢ 0 1 0 0 ⎥ ⎢ 0 0 0⎥ ⎢ ⎥
=⎢ ⎥⎢ ⎥⎢ 1 ⎥ ⎢ 0 cαi −sαi 0 ⎥
⎣ 0 0 1 0 ⎦ ⎣ 0 0 1 di ⎦ ⎣ 0 0 1 0 ⎦ ⎣ 0 sαi cαi 0 ⎦
0 0 01 000 1 0 0 0 1 0 0 0 1
⎡ ⎤⎡ ⎤⎡ ⎤⎡ ⎤
cθi −sθi 0 0 100 0 1 0 0 0 1 0 0 ai
⎢ sθi cθi 0 0 ⎥ ⎢ 0 1 0 0 ⎥ ⎢ 0 cαi −sαi 0 ⎥ ⎢ ⎥
=⎢ ⎥⎢ ⎥⎢ ⎥⎢0 1 0 0 ⎥
⎣ 0 0 1 0 ⎦ ⎣ 0 0 1 di ⎦ ⎣ 0 sαi cαi 0 ⎦ ⎣ 001 0⎦
0 0 01 000 1 0 0 0 1 000 1
⎡ ⎤
cθi −sθi cαi sθi sαi ai cθi
⎢ sθi cθi cαi −cθi sαi ai sθi ⎥
=⎢⎣ 0 sαi
⎥, (2.145)
cαi di ⎦
0 0 0 1
where the four quantities θi , di , ai , and αi are parameters of the ith element and of
the ith joint. These quantities are known as angle, distance, offset distance, and
offset angle, respectively. Since the matrix i−1 Ai is a function of one single variable,
necessarily three of these parameters are constant. In fact, θi is variable for a revolute
joint and di is variable for a prismatic joint, whereas the remaining three are constant.
For the sake of easiness, the following algorithm is introduced.
Algorithm 2.2 Computation of the homogeneous transformations using the D-H

convention
1. Create a table of the parameters ai , di , αi and θi
ai is the distance from the origin oi to the intersection of xi with zi−1 , along the
axis xi . When measuring the distance from the intersection of zi−1 with xi to
the origin oi , the sign of ai is the opposite of the direction of xi .
di is the distance from the origin oi−1 to the intersection of the axes zi−1 and xi ,
along the axis zi−1 .
θi is the angle from xi−1 to xi measured with respect to the axis zi−1 , where the
sense of rotation is obtained by employing the right hand rule.
αi is the angle from zi−1 to zi , with respect to the axis xi , where the sense of
rotation is obtained by employing the right hand rule.
Figure 2.15 shows how to measure θi and αi .
2. Compose the n homogeneous transformation matrices by substituting the above
parameters in the equation:
⎡ ⎤
i−1
Ai = ⎢
⎣ 0 sαi
⎥ (2.146)
cαi di ⎦
0 0 0 1
Fig. 2.15 Rotation angles θi zi−1

and αi zi
αi
zi−1
xi−1
θi
xi
xi
3. Compute the transformation matrix 0 T n = 0 A1 · · · n−1 An , which expresses the

position and the orientation of the end-effector frame with respect to the base
frame.
It is worth to point out that, contrarily to what is expected, the D-H representation
does not require 6 parameters (3 for position and 3 for orientation) to relate each pair
of coordinate frames, but only 4. This is possible due to the constraints imposed on
the frames assignment.
2.2.2.2 Examples
Example
Consider the two degrees of freedom planar robot shown in Fig. 2.16
Let the axes z0 and z1 be normal to the page. The coordinate system ox0 ,y0 ,z0 is
located as shown in the figure, i.e. the origin o0 is established at the intersection
Fig. 2.16 Two degrees of y0 y2

freedom planar robot x2
y1 a2
q2
x1
q1
x0
a1
Table 2.1 D-H parameters of the two degrees of freedom planar manipulator
Joint ai αi di θi
1 a1 0◦ 0 θ1 (t)
2 a2 0◦ 0 θ2 (t)
of the axis z0 with the page, whereas x0 is arbitrarily chosen and the system is
accomplished by locating y0 according to the right hand rule. Once the base system is
fixed, the procedure described in the Denavit-Hartenberg algorithm above is followed
to establish the frames ox1 ,y1 ,z1 and ox2 ,y2 ,z2 . The obtained parameters are shown in
Table 2.1:
Now, the matrices 0 A1 and 1 A2 can be computed by taking into account (2.146)
and the table parameters as
⎡ ⎤ ⎡ ⎤
c1 −s1 0 a1 c1 c2 −s2 0 a2 c2
⎢ s1 c1 0 a1 s1 ⎥ ⎢ s2 c2 0 a2 s2 ⎥
0
A1 = ⎢
⎣ 0 0 1 0 ⎦ and
⎥ 1
A2 = ⎢
⎣ 0 0 1 0 ⎦.
⎥
0 0 0 1 0 0 0 1
The matrix 0 T 1 is simply given by 0 T 1 = 0 A1 , whereas 0 T 2 is computed as

⎡ ⎤⎡ ⎤
c1 −s1 0 a1 c1 c2 −s2 0 a2 c2
⎢ s1 c1 0 a1 s1 ⎥ ⎢ s2 c2 0 a2 s2 ⎥
0
T 2 = 0 A1 1 A2 = ⎢ ⎥⎢
⎣ 0 0 1 0 ⎦⎣ 0 0 1 0 ⎦
⎥
0 0 0 1 0 0 0 1
⎡ ⎤
c1 c2 − s1 s2 −c1 s2 − s1 c2 0 a2 c1 c2 − a2 s1 s2 + a1 c1
⎢ s1 c2 + c1 s2 −s1 s2 + c1 c2 0 a2 s1 c2 + a2 c1 s2 + a1 s1 ⎥
=⎢⎣
⎥
⎦
0 0 1 0
0 0 0 1
⎡ ⎤
c12 −s12 0 a2 c12 + a1 c1
⎢ s12 c12 0 a2 s12 + a1 s1 ⎥
=⎢⎣ 0
⎥,
⎦
0 1 0
0 0 0 1
where s12 = sin(θ1 + θ2 ) and c12 = cos(θ1 + θ2 ). Note that the first two elements on
the fourth column of this matrix are the components of the origin o2 with respect to
the base frame, i.e.
x = a1 c1 + a2 c12
y = a1 s1 + a2 s12 .
Fig. 2.17 Spherical wrist θ6

configuration
z6
z3
θ5
z5
d6
z4
oc
θ4
Table 2.2 D-H parameters for a spherical wrist

Joint ai αi di θi
4 0 −90◦ 0 θ4 (t)
5 0 90◦ 0 θ5 (t)
6 0 0◦ d6 θ6 (t)
In other words, (x, y) are the coordinates of the end effector in the base frame. On
the other hand, the upper-left part of this matrix, i.e. the rotation part of 0 T 2 , gives
the orientation of the frame ox2 ,y2 ,z2 with respect to the base frame.
Example
Consider the spherical wrist configuration shown in Fig. 2.17. Since the spherical
wrist is attached to the robot’s main body made usually up of three joints, then in
the figure the axes are numbered as z3 , z4 and z5 and they intersect each other at the
center of the wrist denoted by oc . The corresponding Denavit-Hartenberg parameters
are summarized in Table 2.2.
It can be easily shown that the joint variables θ4 , θ5 , and θ6 are the Euler angles φ,
θ , and ψ for the Z Y Z configuration, respectively, with respect to the frame ox3 y3 z 3 .
In order to see this compute the matrices 3 A4 , 4 A5 , and 4 A6 , which according to
Table 2.2 are
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
c4 0 −s4 0 c5 0 s5 0 c6 −s6 0 0
⎢ s4 0 c4 0 ⎥ ⎢ s5 0 −c5 0⎥ ⎢ s6 c6 0 0 ⎥
3
A4 = ⎢
⎣ 0 −1 0 0 ⎦
⎥ 4
A5 = ⎢
⎣0
⎥ 5
A6 = ⎢ ⎥
⎣ 0 0 1 d6 ⎦ .
1 0 0⎦
0 0 0 1 0 0 0 1 0 0 0 1
By performing the matrix product, it is

3
R6 3 d 6
3
T 6 = A4 A5 A6 =
3 4 5
0T 1
⎡ ⎤
c4 c5 c6 − s4 s6 −c4 c5 s6 − s4 c6 c4 s5 c4 s5 d6
⎢ s4 c5 c6 + c4 s6 −s4 c5 s6 + c4 c6 s4 s5 s4 s5 d6 ⎥
=⎢
⎣
⎥.
−s5 c6 s5 s6 c5 c5 d6 ⎦
0 0 0 1
By comparing the rotation matrix 3 R6 of 3 T 6 with the Euler angles composition (2.44)
⎡ ⎤
cφ cθ cψ − sφ sψ −cφ cθ sψ − sφ cψ cφ sθ
Rz,φ R y,θ Rz,ψ = ⎣ sφ cθ cψ + cφ sψ −sφ cθ sψ + cφ cψ sφ sθ ⎦ ,
−sθ cψ sθ sψ cθ
it can be seen that θ4 , θ5 , and θ6 are exactly the Euler angles φ, θ , and ψ with respect
to the coordinate frame ox3 y3 z 3 .
Example
Consider the robot manipulator with two revolute joints shown in Fig. 2.18. This
robot has the same kinematic structure of the Furuta pendulum when the parameter
1 is set to zero. The coordinate frames which are also shown in the figure were
Fig. 2.18 Serial manipulator x2

with two revolute joints
y1
x1
y2 z2
1 2
z1 b2
z0
b1
y0
x0
θ2
θ1
Table 2.3 D-H parameters of the two degrees of robot shown in Fig. 2.18
Joint ai αi di θi
1 1 90◦ b1 θ1 (t)
2 2 0◦ b2 θ2 (t)
assigned according to the D-H convention. Notice that the origin of the coordinate
frame x1 y1 z 1 is placed outside of the body of the robot.
The D-H parameters of the robot are shown in Table 2.3 and the homogeneous
transformation matrices are given by
⎡ ⎤ ⎡ ⎤
c1 0 s1 1 c1 c2 −s2 0 2 c2
⎢ s1 0 −c1 1 s1 ⎥ ⎢ s2 c2 0 2 s2 ⎥
0
A1 = ⎢
⎣0
⎥, 1
A2 = ⎢ ⎥.
1 0 b1 ⎦ ⎣0 0 1 b2 ⎦
0 0 0 1 0 0 0 1
Finally, the orientation and end-effector position of the robot are given by
⎡ ⎤
c1 c2 −c1 s2 s1 c1 ( 1 + 2 c2 ) + b2 s1
0
T 2 = 0 A1 1 A2 = ⎣ s1 s2 −s1 s2 −c1 s1 ( 1 + 2 c2 ) − b2 c1 ⎦ .
s2 c2 0 b1 + 2 s2
2.3 Inverse Kinematics
2.3.1 Introduction
It has already been shown how to determine the pose of the end-effector in terms
of the joint variables. In this section, the inverse problem will be studied, i.e. how
to find the joint variables given the pose of the end-effector. This problem is known
as inverse kinematics, which is, in general, more difficult to solve than the direct
kinematics problem. The inverse kinematics problem can be established as follows:
given a 4 × 4 homogeneous transformation by
0
Rd od
0
Hd = ∈ S E(3), (2.147)
0T 1
with 0 Rd ∈ S O(3), find if exists the solution (or multiple solutions) of the equation
0
T n (q1 , . . . , qn ) = 0 H d , (2.148)
2.3 Inverse Kinematics 51
where
0
T n (q1 , . . . , qn ) = 0 A1 · · · n−1 An . (2.149)
Equation (2.148) gives 12 nonlinear equations with n variables to solve, which can
be written as
ti j (q1 , . . . , qn ) = h i j , i = 1, 2, 3, j = 1, 2, 3, 4, (2.150)
where ti j and h i j are the 12 nontrivial elements of 0 T n and 0 H d , respectively. The

solution of these equations in closed form is very difficult and in some cases even
impossible. Therefore, more efficient and systematic techniques were developed by
exploiting the kinematic structures of each manipulator in particular. While the direct
kinematics has always a close solution which can be obtained simply by evaluating
directly (2.146) with the information of the table of D-H parameters, the kinematics
inverse problem might have a single solution, multiple solutions or no solution at all.
Even if this solution is unique, it could be very difficult to obtain given the nonlinear
nature of the inverse kinematics equations.
Of course the solution can be found numerically, but this has the drawback of
high computational costs, which makes it not suitable in a vast number of practical
applications. Instead, a closed form solution is preferred, i.e. a solution of the form
qk = f k (h 11 , . . . , h 34 ), k = 1, . . . , n. (2.151)
The closed form of solutions are preferred not only for their low computational cost,
but because they allow to choose between different valid solutions if necessary.
The existence of solutions of the inverse kinematics problem depends not only
on mathematical considerations but also depends on physical limitations. One can
assume that at least one solution of (2.151) exists if the given pose for the end-effector
lies in the workspace of the particular manipulator, i.e. 0 H d in (2.151) corresponds
to a valid configuration in the dexterous workspace of the manipulator.
2.3.2 Kinematic Decoupling
Although the inverse kinematics problem is very difficult to solve for robots with
many degrees of freedom, when the last three joint axes intersect each other at one
point it is possible to decouple the inverse kinematics problem into two simpler cases,
known in the literature as inverse position and inverse orientation. In other words,
for a manipulator with six or more degrees of freedom with their last three joints
forming a spheric wrist, the inverse kinematics can be solved in two easier steps:
first to find the position of the wrist axes intersection point and then the orientation
of the wrist itself.
Fig. 2.19 Kinematic z4

decoupling
z3 d6
z5
oc
y6 z6
z0 o6
x0
For simplicity, assume that the robot has exactly six degrees of freedom and that
the last three joint axes intersect at a point oc . Then Eq. (2.148) is equivalent to solve
simultaneously
0
R6 (q1 , . . . , q6 ) = 0 Rd (2.152)
o6 (q1 , . . . , q6 ) = od , (2.153)
where od and 0 Rd are the given position and orientation for the end-effector, respec-
tively.
The fact that the axes z4 , z5 , and z6 intersect at oc implies that the origins o4 and
o5 coincide at this position as well. An important consequence is that the movement
of the last three joints does not change at all the position of oc , so that the center of
the wrist depends exclusively on the first three joints.
By taking into consideration that the origin o6 fixed at the end-effector is simply
a translation from oc along z5 by the distance d6 , as shown in Fig. 2.19, it can be
concluded that o6 specified in the frame ox0 ,y0 ,z0 is given by
o6 = oc + d6 z6 . (2.154)
Therefore, to achieve o6 = od it is necessary to comply with
oc = od − d6 0 Rd k. (2.155)
T
Note that 0 Rd k = z6 because k = 0 0 1 . As mentioned before, the position of the
wrist center oc does not depend on the last three joints, so that the first goal is to
determine the values of the first three joints which satisfy (2.155). Recall that the
orientation objective is to drive 0 R6 = 0 Rd , i.e.
0
R6 = 0 Rd = 0 R3 3 R6 , (2.156)
from which one can compute

0 −1 0
3
R6 = R3 Rd = 0 RT3 0 Rd , (2.157)
once the first three joints values have been determined by solving (2.155). The impor-
tance of the spheric wrist is now evident, since the matrix 3 R6 is a function of the
angles θ4 , θ5 , and θ6 with the same composition of the Euler angles ZYZ, as it was
shown in the direct kinematics of the wrist example. Note also that the right hand
side of (2.157) is a product of quantities known at this point. Therefore, to obtain the
solution for the last three joints, it is sufficient to extract the ZYZ Euler angles from
the matrix 3 R6 in (2.157).
2.3.3 Inverse Position
For the common kinematic configurations, a geometric method will be studied to find
the solution to the inverse position problem, i.e. to find the joint variables q1 , q2 , q3
corresponding to the center of the wrist oc given by (2.155). The reason for using
a geometric method instead of an analytic one is, on the one hand, the difficulty to
obtain such analytic solution and, on the other hand, most of the commercial manip-
ulators are of one of the five basic kinematic configurations: Cartesian, cylindrical,
spherical, SCARA, and anthropomorphic, along with a spheric wrist attached to
them. In general, the complexity of the inverse kinematic problem increases with the
number of D-H parameters different from zero.
2.3.3.1 Anthropomorphic Manipulator
Consider the anthropomorphic configuration shown in Fig. 2.20, where the D-H coor-
dinate frames assignment is already shown.
The configuration after the kinematic decoupling is shown in Fig. 2.21.
To apply the geometric method for solving the inverse kinematics, it is convenient
to redraw the manipulator in a configuration in which all the revolute joints have
values different from 0, ±π/2, ±π, . . ., and the prismatic joints have values different
from zero. For the present example, such configuration is displayed in Fig. 2.22.
After driving the manipulator into this convenient configuration, the components
oc , denoted by ocx , ocy , ocz , can be projected on the plane x0 y0 as shown in Fig. 2.23.
In this figure, it can be seen that
θ1 = β + γ . (2.158)
The geometric method basically relies on forming triangles with some of their
sides or angles known, and solve them. In this example, to find β, consider the right
a1 a2 d4 d6
y1
x3 x4 , x5 x6
d2 z1 o1 x1
x2 z3
o2,3 o4,5 o6 z , z
5 6
z2 z4 y6
d1 z0
y0
x0
o0
Fig. 2.20 Anthropomorphic configuration
Fig. 2.21 Anthropomorphic

a1 a2 d4
configuration after kinematic
y1
decoupling
x3
d2 z1 o1 x1
o2,3 oc
z2
d1 z0
y0
x0
o0
Fig. 2.22 Anthropomorphic oc

manipulator with joint
valued different from
0, ±π/2, ±π
α
x3 z3 α
x2
y1
x1
θ2 z2
z1
z0 y0
θ1
x0
Fig. 2.23 Projection on the y0

x0 y0
ocy oc
x1 r
γ
θ1 β
ocx x0
d2
triangle with sides ocx and ocy . Thus, β can be computed as
β = atan2(ocy , ocx ). (2.159)
On the other hand, γ can

be found by forming the triangle with a side given by d2
and hypotenuse r = ± ocx 2 + o2 . This angle can be computed as
cy

γ = atan2 d2 , ± r − d2 = atan2 d2 , ± ocx
2 2 2 + o2 − d 2
cy 2 . (2.160)
Gathering all together, the solution for the first joint is

θ1 = atan2 ocy , ocx + atan2 d2 , ± ocx
2 + o2 − d 2
cy 2 . (2.161)
Notice that this solution exists only if (ocx , ocy ) = (0, 0), i.e. when the wrist center
is not located exactly above the first joint axis. For the manipulator presented in this
example, this case is not possible, because of the offset d2 . Nevertheless, there exist
many manipulators without this offset, for which such situation is possible. This is
known as singular configuration or singularity, and will be addressed in the next
section.
To find the angles θ2 and θ3 , consider the projection of the robot on the plane x1 y1 ,
which is shown in Fig. 2.24.
First, notice that θ3 = α + π/2. Now, by applying the law of cosines
m 2 = (r − a1 )2 + (ocz − d1 )2 = a22 + d42 − 2a2 d4 cos(π − α). (2.162)
Because cos(π − α) = cos(π ) cos(α) + sin(π ) sin(α) = − cos(α), one gets
m 2 − a22 − d42
cos(α) = = D. (2.163)
2a2 d4
Fig. 2.24 Projection on the y1

x1 y1 plane oc
m
d4
ocz − d1
x3 α θ3 α
x2
ϕ
a2
δ θ2
x1
r − a1
Fig. 2.25 Spheric d3 d4

manipulator after kinematic
x2 x3
decoupling
oc
x1 z2 z3
z1
d1
z0
y0
x0
Thus, the solution for the third joint is given by

θ3 = α + π/2 = atan2 ± 1 − D 2 , D . (2.164)
From Fig. 2.24 one can see that θ2 = δ − ϕ. The angle δ can be computed as
δ = atan2 (ocz − d1 , r − a1 ) . (2.165)
To get ϕ one can use α and form a right triangle with sides a2 + d4 cos(α) and
d4 sin(α). Hence, this angle can be computed as
ϕ = atan2 (d4 sin(α), a2 + d4 cos(α)) . (2.166)
Finally, the solution for the second joint is given by
θ2 = atan2 (ocz − d1 , r − a1 ) − atan2 (d4 sin(α), a2 + d4 cos(α)) . (2.167)
Note that there are four valid solutions for the inverse kinematics, as a result of
combining the two solutions given by choosing the signs in (2.161) and in (2.164).
In (2.161), the signs + and − correspond to the right arm and left arm solutions,
while in (2.164) correspond to the elbow down and elbow up solutions, respectively.
Fig. 2.26 Spheric

manipulator in a convenient
d4
configuration for solving the
inverse kinematics x3
oc
d3 z3
o3
x2 β y1
θ 2 z2
β
x1 θ1 x0
z1
d1 z0
y0
x0
Fig. 2.27 Projection of the y0

spheric manipulator on the
x0 y0 plane
ocy oc
θ1
ocx x0
2.3.3.2 Spheric Configuration
Figure 2.25 shows a spheric configuration after kinematic decoupling. Redraw it

with all the revolute joint variables different from 0, ±π/2, ±π and prismatic joint
variables different from zero, as displayed in Fig. 2.26. From the figure it can be seen
that θ2 = β + π/2.
Now, to find the solution for θ1 , it is convenient to project the manipulator on the
x0 y0 plane, as shown in Fig. 2.27.
From the figure, the solution for the first angle is easily found to be
θ1 = atan2(ocy , ocx ), (2.168)

Fig. 2.28 Projection of the y1

spheric manipulator on the
x1 y1 plane x3
d4
β
d3 z3
x2 z2 ocz − d1
θ2 β
x1
r
as long as (ocx , ocy ) = (0, 0). The case (ocx , ocy ) = (0, 0) is a singular point and θ1
can take any value. To find the solution for the second and third joints, it is convenient
to project the robot on the x1 y1 plane, as shown in Fig. 2.28.
In the figure, r is defined by r 2 = ocx
2
+ ocy2
, and by the Pythagorean theorem it is
(d3 + d4 )2 = ocx
2
+ ocy
2
+ (ocz − d1 )2 , (2.169)
from which the solution for the third joint is obtained as

d3 = 2 + o2 + (o − d )2 − d .
ocx cy cz 1 4 (2.170)
Finally, the solution for the second joint is

θ2 = β + π/2 = atan2 ocz − d1 , ocx cy + π/2.
2 + o2 (2.171)
2.4 Differential Kinematics
In this section, the relationship between joint velocities and end-effector veloci-
ties will be developed. Mathematically speaking, the direct kinematics equations
define a function that maps the joint variables to the position and orientation of the
end-effector in the Cartesian space. The relationship between the velocities is thus
naturally given by taking the time derivative of the direct kinematics function. Since
it depends only on the joint positions, by using the chain rule the relationship between
the joint and Cartesian velocities is given by a matrix, which is called the Jacobian
matrix, or simply Jacobian. This matrix is one of the most important and useful
quantities in robotics, with a great number of applications from dynamic modeling
to manipulability analysis, path planing, force control, visual servoing, etc. The same
approach can be used to relate the velocity of any point on the robot and the joint
velocities.
2.4 Differential Kinematics 59
In the most general case, the Cartesian linear and angular velocities can be rep-
resented by a vector of dimension 6. In this section, first for a manipulator with
n joints, the corresponding 6 × n configuration-dependent Jacobian matrix will be
developed, which maps the joint velocities to end-effector Cartesian velocities. Sec-
ond, the same procedure will be employed to compute the Jacobian for any arbitrary
point on the robot. Later, the configurations which make the Jacobian to lose rank,
known as singularities will be studied. Finally, the relationship between the joint
and end-effector accelerations will be discussed.
2.4.1 Analytic Jacobian
There are basically two important types of Jacobians that are useful for robotics, the
analytic Jacobian and the geometric Jacobian. To derive the first one, let us bundle
the Cartesian position and the orientation of the end-effector in the vector
0
dn
x= , (2.172)
φ
where φ ∈ R p is a parametrization of the orientation. Recall that the direct kinematics

is expressed as a homogeneous transformation of the form
⎡0 ⎤
Rn (q) 0 d n (q)
0
T n (q) = ⎣ ⎦. (2.173)
T
0 1
Thus, the vector φ is just a parametrization of the rotation matrix 0 Rn (q). For the
general case of the 3D Cartesian space, a vector-valued function f (q) : Rn → R3+ p
of the end-effector pose in terms of the n joint positions can be constructed, where p
is the dimension of the orientation parametrization, e.g. p = 3 for Euler angles and
p = 4 for unit quaternions. In the particular case of planar robots, the position and
orientation can be described by a vector φ ∈ R3 . Therefore, the direct kinematics
can be expressed as
x = f (q). (2.174)
By taking the time derivative of this equation, one has

0
ḋ n ∂f (q)
ẋ = = q̇ = J A (q)q̇. (2.175)
φ̇ ∂q
The quantity J A (q) = ∂f (q)/∂q ∈ R(3+ p)×n is the analytic Jacobian of the manipu-
lator.
Fig. 2.29 Three degrees of y0

freedom planar robot
q3
φ
q2
q1
x0
Example
Consider the 3 degrees of freedom planar manipulator shown in Fig. 2.29.

After a standard D-H assignment, it is not difficult to obtain the direct kinematics
in the form
⎡ ⎤
c123 −s123 0 a1 c1 + a2 c12 + a3 c123
⎢ −s123 c123 0 a1 s1 + a2 s12 + a3 s123 ⎥
0
T3 = ⎢
⎣ 0
⎥.
⎦ (2.176)
0 1 0
0 0 0 1
For representing the position, it is sufficient to take the first two components
of the end-effector position vector o3 , i.e. 0 d 3 . On the other hand, the angle φ can
be employed to represent the orientation, as shown in Fig. 2.29. This angle can be
computed as φ = q1 + q2 + q3 and it is simply the angle between the end-effector
and the horizontal line. Now, the direct kinematics can be written as
⎡ ⎤ ⎡ ⎤
ox a1 c1 + a2 c12 + a3 c123
x = ⎣ oy ⎦ = ⎣ a1 s1 + a2 s12 + a3 s123 ⎦ = f (q). (2.177)
φ q1 + q2 + q3
Finally, the analytic Jacobian can be computed as

⎡ ⎤
−a1 s1 − a2 s12 − a3 s123 −a2 s12 − a3 s123 −a3 s123
∂f (q) ⎣
J A (q) = = a1 c1 + a2 c12 + a3 c123 a2 c12 + a3 c123 a3 c123 ⎦(2.178)
.
∂q 1 1 1
In the above example, the computation of the analytic Jacobian was quite easy,
since the direct kinematics function (2.177) is rather simple. In general, the compu-
tation of the analytic Jacobian is more difficult for non planar robots with revolute
joints, because the function representing the orientation part is not as simple as in
the example.
2.4.2 Geometric Jacobian
In contrast with the analytic Jacobian, the geometric Jacobian relates the joint veloc-
ities q̇(t) with the linear and angular velocities of the end-effector. Let
0
vn = 0 ḋ n (2.179)
denote the end-effector linear velocity and let 0 ωn be its angular velocity. As explained
in Sect. 2.1.7, this angular velocity is related to the rotation matrix by

S 0 ωn = 0 Ṙn 0 RTn . (2.180)
The objective is to find expressions of the form

0
vn = J v q̇ (2.181)
0
ωn = J ω q̇, (2.182)
where J v and J ω are 3 × n matrices. Obviously, (2.181) and (2.182) can be written
in compact form as
0
vn
ẋ = = 0 J n q̇, (2.183)
0
ωn
where ẋ bundles both linear and angular end-effector velocities and 0 J n is composed
as

Jv
0
Jn = . (2.184)
Jω
The matrix 0 J n above is the geometric Jacobian of the manipulator, or simply Jaco-
bian. Notice that 0 J n is always a 6 × n matrix, where n is the number of manipulator
joints, as usual. However, if n < 6 it is customary to choose only those rows of 0 J n
which are more relevant in order to have a n × n Jacobian. In what follows, the rela-
tionship between joint velocities and end-effector velocities will be studied in order
to develop a method to compute the Jacobian. For simplicity’s sake instead of 0 J n
throughout the book the notation J or J(q) will be employed.
2.4.2.1 Linear Velocity
The end-effector linear velocity is simply 0 ḋ n , which is given by the chain rule as

n
∂ 0dn
0
ḋ n = q̇i . (2.185)
i=1
∂qi
Accordingly, the ith column of J v is given by
∂ 0dn
J vi = .
∂qi
The above expression can be interpreted as the value of the end-effector linear velocity
when q̇i = 1, and q̇ j = 0, with j = i, for i, j = 1, . . . , n. In other words, the ith
column of J v can be generated by making all the joint velocities zero, except for
that of the ith joint, which is made exactly one. The joints can be either prismatic or
revolute and, since their contribution to the motion of the end-effector is different,
will be analyzed separately as follows.
Prismatic joints
If the ith joint is prismatic, then 0 R j is independent of qi = di , for all j. Let us
express 0 d n as
0
d n = 0 d i−1 + 0 Ri−1 i−1 d n . (2.186)
If all joints are fixed except for the ith, it is
∂ 0dn
0
ḋ n = q̇i
∂qi
∂ 0 d i−1 ∂ 0 Ri−1 i−1 d n
= q̇i + q̇i . (2.187)
∂qi ∂qi
Since 0 d i−1 and 0 Ri−1 do not depend on qi , one has
∂ i−1
dn
0
ḋ n = 0 Ri−1 q̇i = 0 Ri−1 i−1 ḋ n . (2.188)
∂qi
Now, i−1 ḋ n = q̇i k since it is a prismatic joint and the velocity is expressed with
respect to the frame i − 1 and as usual k = [0 0 1]T . Thus, it is satisfied
0
ḋ n = q̇i 0 Ri−1 k = q̇i zi−1 , (2.189)
where zi represents the orientation of the z-axis of the coordinate system oxi ,yi ,zi but
expressed in ox0 ,y0 ,z0 . Since the ith column of J v is obtained when all joint velocities
Fig. 2.30 Effect of a joint i

prismatic joint motion on the xn
end-effector linear velocity zi−1
q̇i on zn
z0 yn
y0
x0
o0
are zero but q̇i = 1, one can conclude that such column is given by
∂ 0dn
j vi = = zi−1 . (2.190)
∂qi
The contribution of a prismatic joint motion in the end-effector linear velocity is

graphically represented in Fig. 2.30.
Revolute joints
Consider now the case in which the ith joint is revolute, for which it is still valid
0
d n = 0 d i−1 + 0 Ri−1 i−1 d n , (2.191)
which by a common vectorial subtraction can be expressed as

0
d n − 0 d i−1 = 0 Ri−1 i−1 d n . (2.192)
Since the motion of the ith joint only affects the frames i, . . . , n, then both 0 d i−1
and 0 Ri−1 are constant in such case. Hence, the velocity of the end-effector is given
by
0
ḋ n = 0 Ri−1 i−1 ḋ n . (2.193)
Now, since the motion of the ith joint is composed of a rotation over the zi−1 axis by
the quantity qi , the resulting linear velocity on the end-effector is
i−1
ḋ n = q̇i k × i−1 d n , (2.194)
as can be seen in Fig. 2.31.

Therefore, from (2.193) it is

0
ḋ n = 0 Ri−1 q̇i k × i−1 d n
= q̇i 0 Ri−1 k × 0 Ri−1 i−1 d n

= q̇i zi−1 × 0 d n − 0 d i−1 , (2.195)
Fig. 2.31 Effect of a 0

ḋn xn
revolute joint motion on the
end-effector linear velocity
0 −1 on
0 dn − di zn
q̇i yn
z0
y0 oi−1
zi−1
joint i
o0 x0
where (2.192) has been used. Notice that it is also employed (2.100), i.e. R(a × b) =
Ra × Rb. Accordingly, for this case the ith column of J v is given by
∂ 0dn
jvi = zi−1 × 0 d n − 0 d i−1 . (2.196)
∂qi
Summarizing, the upper part of the Jacobian J v is given by

J v = jv1 · · · j vn , (2.197)
where the ith column jvi is given by

zi−1 if joint i is prismatic
j vi = (2.198)
zi−1 × 0 d n − 0 d i−1 if joint i is revolute
2.4.2.2 Angular Velocity
To derive the procedure for obtaining the relation between joint velocities and the
end-effector angular velocity, first recall (2.137) from Sect. 2.1.7, i.e.
0
ωn = 0 ω1 + 0 R1 1 ω2 + 0 R2 2 ω3 + · · · + 0 Rn−1 n−1 ωn ,
which basically states that angular velocities can be directly added as long as they are
expressed in the same frame. Following this, it is possible to obtain the end-effector
angular velocity with respect to the base frame by expressing the angular velocity of
each element with respect to such frame and then adding these velocities. If the ith
joint is a revolute one, then the rotation axis is zi−1 and the rotation angle is qi . Hence,
the angular velocity of the ith element, expressed in the i − 1 frame is given by
i−1
ωi = q̇i k. (2.199)
On the other hand, if the ith joint is prismatic, then the motion of the ith frame with
respect to the (i − 1)th frame is a linear translation and thus
i−1
ωi = 0. (2.200)
As a result, the angular velocity of the end-effector 0 ωn in the base frame is given
by

n
0
ωn = ρ1 q̇1 k + ρ2 q̇2 0 R1 k + · · · + ρn q̇n 0 Rn−1 k = ρi q̇i zi−1 , (2.201)
i=1
where
zi−1 = 0 Ri−1 k, (2.202)
where ρi , i = 1, . . . , n is equal to 1 if the joint is revolute and equal to 0 if it is

prismatic. Clearly,
⎡ ⎤
0
z0 = k = ⎣ 0 ⎦ .
1
Thereupon, the lower part J ω of the Jacobian (2.184), is given by

J ω = ρ1 z0 · · · ρn zn−1 . (2.203)
Putting it all together, the manipulator Jacobian has the form

0
J n = J = j1 · · · jn , (2.204)
where the ith column ji of J can be computed by

⎡ ⎤
zi−1 × (on − oi−1 )
ji = ⎣ ⎦, (2.205)
zi−1
if the ith joint is revolute and by

⎡ ⎤
zi−1
ji = ⎣ ⎦, (2.206)
0
if the ith joint is prismatic. These equations are useful to find the geometric Jacobian
of any manipulator for which the Denavit-Hartenberg methodology is valid, for which
such computations are very straightforward since all quantities are available from
the D-H algorithm. Finally, it is worth to point out that the procedure above can be
mimicked to obtain the linear and angular velocity of any element of the manipulator
different from the end-effector.
Example
Consider the 3 degrees of freedom planar manipulator of example of Sect. 2.4.1 dis-
played in Fig. 2.29. By following the D-H algorithm, one can find the transformation
matrices
⎡ ⎤ ⎡ ⎤
c1 −s1 0 a1 c1 c12 −s12 0 a1 c1 + a2 c12
⎢ −s1 c1 0 a1 s1 ⎥ 0 ⎢ −s12 c12 0 a1 s1 + a2 s12 ⎥
0
T1 = ⎢ ⎥
⎣ 0 0 1 0 ⎦ , T2 = ⎣ 0
⎢ ⎥ , and
⎦
0 1 0
0 0 0 1 0 0 0 1
⎡ ⎤
c123 −s123 0 a1 c1 + a2 c12 + a3 c123
⎢ −s123 c123 0 a1 s1 + a2 s12 + a3 s123 ⎥
0
T3 = ⎢⎣ 0
⎥.
⎦
0 1 0
0 0 0 1
Since the first joint is revolute, the first column of J v is computed as

j v1 = z0 × 0 d 3 − 0 d 0
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
0 a1 c1 + a2 c12 + a3 c123 −a1 s1 − a2 s12 − a3 s123
= ⎣0⎦ × ⎣ a1 s1 + a2 s12 + a3 s123 ⎦ = ⎣ a1 c1 + a2 c12 + a3 c123 ⎦ .
1 0 0
In the same manner, the remaining columns are given by

⎡ ⎤ ⎡ ⎤
−a2 s12 − a3 s123 −a3 s123
j v2 = ⎣ a2 c12 + a3 c123 ⎦ , and j v3 = ⎣ a3 c123 ⎦ . (2.207)
0 0
On the other hand, the columns of the lower part J ω are simply
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
0 0 0
jω1 = z0 = ⎣0⎦ , j ω2 = z1 = ⎣0⎦ , and j ω3 = z2 = ⎣0⎦ .
1 1 1
Finally, the complete Jacobian matrix is

⎡ ⎤
−a1 s1 − a2 s12 − a3 s123 −a2 s12 − a3 s123 −a3 s123
⎢ a1 c1 + a2 c12 + a3 c123 a2 c12 + a3 c123 a3 c123 ⎥
⎢ ⎥
⎢ 0 0 0 ⎥
J(q) = ⎢
⎢
⎥. (2.208)
⎢ 0 0 0 ⎥ ⎥
⎣ 0 0 0 ⎦
1 1 1
As expected, the upper part, the linear velocity of the end-effector, is the same as
that in (2.178), if the third row is obviated. However, despite the last row of (2.178)
and (2.208) are the same, this is not true in general.
It is important to notice that in (2.175) and in (2.183) the same notation ẋ has
been employed, though representing different quantities. Although, the first three
components of both Jacobians are the same, the last three ones are in general different.
0
ωn in (2.183) represents the angular velocity of the end-effector frame with respect
to the base and it has a precise physical meaning. However, its integral over time
does not have it unless the rotation takes place around one single axis, where then it
represents precisely the angle of rotation. On the other hand, φ̇ in (2.175) does not
own any physical meaning, while φ represents some orientation parameters, e.g. the
Euler angles.
Example

For the unaware reader it may be surprising that 0 ωn dt angular velocities does not
have a physical meaning in general since when rotating around one single axis it
represents the angle of rotation. To see this, suppose the following angular velocity
is chosen from 0 ≤ t ≤ 1 s:
⎡ ⎤
0
rad
ω1 = ⎣ 0 ⎦
π s
2
This is equivalent to a basic rotation around the z0 axis by 90◦ , i.e. Rz,90◦ . Then from
1 < t ≤ 2 s choose
⎡ ⎤
0
⎣ π ⎦ rad
ω2 = 2 ,
0 s
which is equivalent to a basic rotation around the y0 axis by 90◦ , i.e. R y,90◦ . Therefore,
the total rotation is given by
0
R1 = R y,90◦ Rz,90◦ . (2.209)
Now, suppose it is chosen first ω2 for 0 ≤ t ≤ 1 s, and ω1 for 1 < t ≤ 2 s. For that
case the total rotation is given by
0
R1 = Rz,90◦ R y,90◦ . (2.210)
Clearly
0
R1 = 0 R1 . (2.211)
However, by computing the integral over 0 ≤ t ≤ 2 one gets

⎡ ⎤
0
1 2 1 2 ⎢ ⎥
⎢π ⎥
ω1 dt + ω2 dt = ω2 dt + ω1 dt = ⎢
⎢2
⎥.
⎥
⎣ ⎦
0 1 0 1
π
2
Therefore, the result is the same and it cannot be related at all with (2.211).
2.4.3 Singularities
The Jacobian matrix J(q) of dimension 6 × n, defines the mapping
ẋ = J(q)q̇, (2.212)
from the joint velocities vector q̇ to the end-effector velocities one

v
ẋ = .
ω
This equation represents also a linear transformation between differentials dx and

dq, as follows
dx = J(q)dq. (2.213)
One can see such differentials as possible directions of motions in Rn and in R6 ,

respectively. Since the Jacobian is a function of the joint position vector q, it is
important to analyze those configurations for which J(q) loses rank. Such configura-
tions are called singularities or singular configurations. The relevance of analyzing
them arises due to several reasons:
• They represent configurations from which certain directions of motion cannot be
reached.
References 69
• In a singular configuration, bounded end-effector velocities can correspond to

unbounded joint velocities.
• Most of the times, singularities are found in the border of the robot workspace (max-
imum reachable points).
• In a singular configuration, bounded forces and moments at the end-effector can
correspond to unbounded joint torques and forces.
• At some singularities, points in the manipulator workspace are no longer reachable
under slight modifications of the robot kinematic parameters.
• Near singular configurations, a unique solution for the inverse kinematics might
not exist or there could be an infinite number of solutions.
Example
Consider again the example of the three degrees of freedom planar manipulator
shown in Fig. 2.29. Since its analytic Jacobian (2.178) is a square matrix, its singular
points can be obtained by computing its determinant given by
det (J A (q)) = a1 a2 sin (q2 ) . (2.214)
Hence, the analytic Jacobian loses rank when q2 = kπ/2, for any integer k. These
points correspond to the configurations for which the arm formed by the first two
links of the manipulator is either full-stretched or full-contracted. As mentioned
above, such configurations correspond to points on the border of the manipulator
workspace.
References
Beer FP, Johnston ER Jr, Mazurek DF, Cornwell PJ (2013) Vector mechanics for engineers: statics
and dynamics, 10th edn. McGraw-Hill, USA
Hill, USA
Press, Boca Raton, FL
Sciavicco L, Siciliano B (2000) Modeling and control of robot manipulators, 2nd edn. Springer,
London
Siciliano B, Villani L (1999) Robot force control. Kluwer Academic Publishers, The Netherlands
Strang G (2006) Linear algebra and its applications, 4th edn. Thompson Brooks, USA
Chapter 3
Dynamics of Rigid Robot Manipulators
Abstract As for most physical systems, control-observer design of robot manip-

ulators can take advantage of the knowledge of a dynamic model. It turns out that
their usual analytical description owns many structural properties due to the phys-
ical characteristics of mechanical systems, while some other properties arise from
the modeling methodology employed. In this chapter, the modeling of n degrees of
freedom rigid robot manipulators on the basis of the Lagrange’s equations of motion
is briefly discussed. Several notable properties are presented and their impact on
control-observer design is underlined.
3.1 Dynamic Modeling of Rigid Robot Manipulators
The equations of motion of rigid robot manipulators are a set of analytical equations
to describe their dynamics that are useful for many purposes, e.g. control-observer
design, computer simulation, parameter estimation, robot design, etc. It is possi-
ble to obtain a dynamic model by using physical laws leading to a set description
based on the number of joints of the manipulator as well as geometrical and inertial
characteristics of the links. Perhaps the two most employed methodologies are the
Euler-Lagrange and the Newton-Euler equations. In this chapter the former will be
presented in detail.
3.1.1 Euler-Lagrange Equations of Motion
The general equations of motion of a robot manipulator can be obtained by the

direct application of the Euler-Lagrange formulation for non conservative systems,
resulting in

d ∂L ∂L
− = τi i = 1, . . . , n, (3.1)
dt ∂ q̇i ∂qi

https://doi.org/10.1007/978-3-030-85980-0_3
72 3 Dynamics of Rigid Robot Manipulators
where
L = Lagrangian function = kinetic energy K − potential energy P
K = total kinetic energy of the manipulator
P = total potential energy of the manipulator
qi = generalized coordinate of the manipulators
τi = generalized force or torque applied at the ith joint to move the ith link.
For robot manipulators the generalized coordinates are joint angles or displace-
ments, depending on whether the ith joint is revolute or prismatic.
3.1.2 Kinetic Energy
Let the ith link of a robot be made up of a set of particles and let ρi be the mass
density, for i = 1, . . . , n. Let Bi be the tridimensional space region occupied by the
link, such that

ρi (x, y, z)dx dy dz = m i , (3.2)
Bi
where m i is the total mass of the ith link. The corresponding kinetic energy is then
given by

1
Ki = viT (x, y, z)vi (x, y, z)ρi (x, y, z)dx dy dz
2
Bi

1
= viT (x, y, z)vi (x, y, z)dm, (3.3)
2
Bi
where dm stands for the infinitesimal mass of the particle located at the coordinates
(x, y, z). When a body moves in the space not all particles move with the same
velocities. Therefore, it is more convenient to use the link’s center of mass, which
can be expressed as:

1
0
r̄i = 0
ri dm, (3.4)
mi
Bi
where 0 r̄i stands for the position of the ith center of mass and 0 ri represents any point
on the object. Note that 0 r̄i is fixed on the ith link and therefore it is independent of
the mass differential, which means that the previous equation is equivalent to

0
r̄i − 0 ri dm = 0, (3.5)
Bi
3.1 Dynamic Modeling of Rigid Robot Manipulators 73
zci
xci ci
ri
nk
- 1 ) - th li
(i yci
i-th link
0
r̄ i
0
ri
z0
y0
x0
Fig. 3.1 An extra coordinate frame can be attached to the center of mass of the ith link
because

0
r̄i dm − 0
ri dm = 0
r̄ m − 0
ri dm = 0. (3.6)
i i
Bi Bi 0
r̄i does not depend on m i Bi
This property of the center of mass allows to attach a new coordinate frame in the
ith link as shown in Fig. 3.1.
When the link moves, the velocity of a point on it with respect to the base
coordinate frame and expressed on the same coordinate frame can be computed
by using (2.119)
0
vi = 0 v̄i + 0 ω̄i × 0 Rci ci ri , (3.7)
where ci ri is the position of the same point but with respect to the center of mass
frame. Note that it is also possible to express 0 vi with respect to the coordinate frame
fixed at the ith center of mass, oxci ,yci ,zci , just by multiplying by the corresponding
rotation matrix ci R0 = 0 RTci . Then, the velocity of the particle located at 0 ri with
respect to the base frame but express in oxci ,yci ,zci is given by
0
ci
vi = ci R0 0 vi = 0 RTci 0 vi = 0 RTci v̄i + 0 ω̄i × 0 Rci ci ri

= 0 RTci 0 v̄i + 0 RTci 0 ω̄i × 0 RTci 0 Rci ci ri

= ci R0 0 v̄i + ci R0 0 ω̄i × ci R0 0 Rci ci ri
= ci v̄i + ci ω̄i × ci ri . (3.8)
By recalling that the cross product can be carried out by using a skew symmetric
matrix one gets
ci
vi = ci v̄i + S(ci ω̄i )ci ri . (3.9)
This relationship is useful because the kinetic energy of the link has to be the same
in any frame, so that by substituting (3.9) in (3.3) yields

1 ci T ci
Ki = v̄i + S(ci ω̄i )ci ri v̄i + S(ci ω̄i )ci ri dm
2
Bi

1
= ci T ci
v̄i v̄i + ci v̄iT S(ci ω̄i )ci ri + ci riT S(ci ω̄i )Tci v̄i
2
Bi
+ci riT S(ci ω̄i )T S(ci ω̄i )ci ri dm

= Ki1 + Ki2 + Ki3 + Ki4 , (3.10)
where the last four terms represent the four integrals to be solved. The first one can
be calculated as

1 ci T ci 1
Ki1 = v̄i v̄i dm = m i ci v̄iT ci v̄i , (3.11)
2 2
Bi
where it has been taken advantage of the fact that ci v̄i is independent of the link mass.
Ki1 represents the translational part of the kinetic energy. As to Ki2 it is

1 1
Ki2 = v̄i S( ω̄i )ci ri dm
ci T ci
= ci v̄iT S(ci ω̄i ) ci
ri dm = 0, (3.12)
2 2
Bi Bi
because

ci
ri dm = m i ci r̄i = 0, (3.13)
Bi
in view of the fact that ci r̄i is independent of the mass and it represents the i center
of mass position with respect to the frame fixed precisely at that center of mass. For
the same reason one also has

1
Ki3 = ri S ( ω̄i )ci v̄i dm
ci T T ci
= 0. (3.14)
2
Bi
It remains to compute Ki4

1
Ki4 = ri S ( ω̄i )S(ci ω̄i )ci ri dm,
ci T T ci
(3.15)
2
Bi
which is not as direct as the first three terms. First of all consider rewriting

1
Ki4 = Tr S(ci ω̄i )ci ri ci riT ST (ci ω̄i ) dm
2
Bi
⎛ ⎞

1 ⎝ ci
= Tr S( ω̄i ) ci
ri ci riT dmST (ci ω̄i )⎠
2
Bi
1
= Tr S(ci ω̄i )J i ST (ci ω̄i ) , (3.16)
2
where it has been taken advantage that ci ω̄i is associated to the center of mass position
and thus it can be taken outside the integral, while ci ri represents the position of all
particles of the link. The matrix J i ∈ R3×3 is given by

Ji = ci
ri ci riT dm, (3.17)
Bi
or
⎡ 2 ⎤ ⎡ ⎤
xi dm xi y2 i dm xi z i dm j11 j12 j13
J i = ⎣ xi yi dm yi dm yi z i dm ⎦ = ⎣ j12 j22 j23 ⎦ . (3.18)
xi z i dm yi z i dm z i2 dm j13 j23 j33
Say that ci ω̄i = [ω̄1i ω̄2i ω̄3i ]T , and recall

⎡ ⎤
0 −ω̄3i ω̄2i
S(ci ω̄i ) = ⎣ ω̄3i 0 −ω̄1i ⎦ , (3.19)
−ω̄2i ω̄1i 0
so that S(ci ω̄i )J i ST (ci ω̄i ) can be computed as

⎡ ⎤⎡ ⎤⎡ ⎤
0 −ω̄3i ω̄2i j11 j12 j13 0 ω̄3i −ω̄2i
⎣ ω̄3i 0 −ω̄1i ⎦ ⎣ j12 j22 j23 ⎦ ⎣ −ω̄3i 0 ω̄1i ⎦
−ω̄2i ω̄1i 0 j13 j23 j33 ω̄2i −ω̄1i 0
⎡ ⎤⎡ ⎤
−ω̄3i j12 + ω̄2i j13 −ω̄3i j22 + ω̄2i j23 −ω̄3i j23 + ω̄2i j33 0 ω̄3i −ω̄2i
= ⎣ ω̄3i j11 − ω̄1i j13 ω̄3i j12 − ω̄1i j23 ω̄3i j13 − ω̄1i j33 ⎦ ⎣ −ω̄3i 0 ω̄1i ⎦
−ω̄2i j11 + ω̄1i j12 −ω̄2i j12 + ω̄1i j22 −ω̄2i j13 + ω̄1i j23 ω̄2i −ω̄1i 0
= (ci ω̄i )
where the elements of (ci ω̄i ) are given by
11 = −ω̄3i (−ω̄3i j22 + ω̄2i j23 ) + ω̄2i (−ω̄3i j23 + ω̄2i j33 )
12 = ω̄3i (−ω̄3i j12 + ω̄2i j13 ) − ω̄1i (−ω̄3i j23 + ω̄2i j33 )
13 = −ω̄2i (−ω̄3i j12 + ω̄2i j13 ) + ω̄1i (−ω̄3i j22 + ω̄2i j23 )
21 = −ω̄3i (ω̄3i j12 − ω̄1i j23 ) + ω̄2i (ω̄3i j13 − ω̄1i j33 )
22 = ω̄3i (ω̄3i j11 − ω̄1i j13 ) − ω̄1i (ω̄3i j13 − ω̄1i j33 )
23 = −ω̄2i (ω̄3i j11 − ω̄1i j13 ) + ω̄1i (ω̄3i j12 − ω̄1i j23 )
32 = ω̄3i (−ω̄2i j11 + ω̄1i j12 ) − ω̄1i (−ω̄2i j13 + ω̄1i j23 )
33 = −ω̄2i (−ω̄2i j11 + ω̄1i j12 ) + ω̄1i (−ω̄2i j12 + ω̄1i j22 ).
Therefore
Tr(S(ci ω̄i )J i ST (ci ω̄i )) = −ω̄3i (−ω̄3i j22 + ω̄2i j23 ) + ω̄2i (−ω̄3i j23 + ω̄2i j33 )
+ω̄3i (ω̄3i j11 − ω̄1i j13 ) − ω̄1i (ω̄3i j13 − ω̄1i j33 )
−ω̄2i (−ω̄2i j11 + ω̄1i j12 ) + ω̄1i (−ω̄2i j12 + ω̄1i j22 )
⎡ ⎤⎡ ⎤
T j22 + j33 − j12 − j13 ω̄1i
= ω̄1i ω̄2i ω̄3i ⎣ − j12 j11 + j33 − j23 ⎦ ⎣ ω̄2i ⎦
− j13 − j23 j11 + j22 ω̄3i

Ii
I i ∈ R3×3 is an inertia matrix, which according to (3.18) is given by

⎡ ⎤
yi2 + z i2 dm − xi yidm − xi z i dm
I i = ⎣ − xi yi dm xi2+ z i2 dm − 2 yi z i2dm

⎦
− xi z i dm − yi z i dm xi + yi dm
⎡ ⎤
Ixxi − xi yi dm − xi z i dm
= ⎣ − xi yi dm Iyyi − yi z i dm ⎦
− xi z i dm − yi z i dm Izzi
⎡ ⎤
Ixxi −Ixyi −Ixzi
= ⎣ −Ixyi Iyyi −Iyzi ⎦ , (3.20)
−Ixzi −Iyzi Izzi
where Ixxi , Iyyi , Izzi are moments of inertia and Ixzi , Ixyi , Iyzi are products of
inertia computed with respect to oxci ,yci ,zci . From (3.16) it is
1 ci T ci
Ki4 = ω̄ I i ω̄i , (3.21)
2 i
which represents the rotation part of the kinetic energy of the ith link. After (3.11)
and (3.21) the total kinetic energy is given by
1 ci T ci 1
Ki = m i v̄i v̄i + ci ω̄iT I i ci ω̄i
2 2
1 ci 0 T ci 0 1 ci 0 T ci 0
= m i R0 v̄i R0 v̄i + R0 ω̄i I i R0 ω̄i
2 2
1 1
= m i 0 v̄iT ci RT0 ci R0 0 v̄i + 0 ω̄iT ci RT0 I i ci R0 0 ω̄i . (3.22)
2 2
Finally, it is
1 0 T0 1
Ki = m i v̄i v̄i + 0 ω̄iT 0 Rci I i 0 RTci 0 ω̄i . (3.23)
2 2
To compute the lineal and angular velocities of the ith center of mass, Jacobian
matrices can be computed to get
0
v̄i = J vci (q)q̇ and 0
ω̄i = J ωci (q)q̇. (3.24)
This way, the manipulator’s total kinetic energy is given by

n
K= Ki , (3.25)
i=1
which from (3.23) and (3.24) becomes
1 T
K= q̇ H(q)q̇, (3.26)
2
where

n
H(q) = m i J Tvci (q)J vci (q) + J Tωci (q)0 Rci (q)I i 0 RTci (q)J ωci (q) . (3.27)
i=1
3.1.3 Potencial Energy
Let P be the total potential energy of a robot manipulator and Pi the potential energy
of the ith link, which can straightforwardly be computed as
Pi = −m i ḡT0 r̄i , (3.28)
for i = 1, 2, . . . , n, and
⎡ ⎤
gx
ḡ = ⎣ g y ⎦
gz
is a gravity vector expressed in the base coordinate frame. For example, for the link
depicted in Fig. 3.1 it is
⎡ ⎤
0
ḡ = ⎣ 0 ⎦ ,
−g0
where g0 = 9.8062 m/s2 is the gravitational constant. Finally

n
n
P= Pi = − m i ḡT0 r̄i . (3.29)
i=1 i=1
3.2 Equations of Motion of a Robot Manipulator
3.2.1 Generalized Model
By taking into account (3.26) and (3.29), the Lagrangian function L = K − P is

given by
1 n
L = q̇T H(q)q̇ + m i ḡT0 r̄i . (3.30)
2 i=1
Express (3.1) in matrix form to get1
d ∂L ∂L
− = τ, (3.31)
dt ∂ q̇ ∂q
where τ ∈ Rn is the generalized force/torque vector acting at the joints.
1Technically, ∂(·)
∂ q̇ produces a row vector; however, for simplicity’s sake it is considered here a
column vector although this is an abuse of notation.
3.2 Equations of Motion of a Robot Manipulator 79
The next step is to compute a model in matrix form by developing the left hand
side of (3.31). Beginning with ∂L
∂q
, from (3.30) and (3.31) one gets
n
∂L ∂ 1 ∂ T
− =− m i ḡ r̄i −
T0
q̇ H(q)q̇ . (3.32)
∂q ∂q i=1 2 ∂q
The first term of the right hand side of (3.32) represents the gravity vector g(q) ∈ Rn ,
whose jth element can be obtained by:
n
∂
gj = − m i ḡ r̄i ,
T0
(3.33)
∂q j i=1
and that can be written in a compact form as
∂ P
g(q) = . (3.34)
∂q
The second term of the right hand side of (3.32) belongs to the vector of Coriolis and
centrifugal torques to be obtained later. Prior to it, the first part of (3.31) is developed:
d ∂L d
= H(q)q̇ = H(q)q̈ + Ḣ(q)q̇. (3.35)
dt ∂ q̇ dt
The vector H(q)q̈ is already in its final form, while Ḣ(q)q̇ ∈ Rn belongs as well to the
vector of Coriolis and centrifugal torques denoted here by hc (q, q̇) ∈ Rn . Therefore,
it is
1 ∂ T
hc (q, q̇) = Ḣ(q)q̇ − q̇ H(q)q̇ . (3.36)
2 ∂q
Using (3.36) to compute hc (q, q̇) is not practical. Instead it is a common and better
practice to write hc (q, q̇) as
hc (q, q̇) = C(q, q̇)q̇, (3.37)
for a matrix C(q, q̇) ∈ Rn×n . To find the elements of this matrix consider that the ith
element of hc (q, q̇), h ci is given by

n
1 ∂h k j
n n
h ci = ḣ i j q̇ j − q̇k q̇ j
j=1
2 k=1 j=1 ∂qi
n
n ∂h i j 1 ∂h k j
n n
= q̇k q̇ j − q̇k q̇ j
j=1 k=1
∂qk 2 k=1 j=1 ∂qi
n n
∂h i j 1 ∂h k j
= − q̇k q̇ j . (3.38)
k=1 j=1
∂qk 2 ∂qi
In view of the fact that

⎛ ⎞

n
n
∂h i j 1 ⎝ ∂h i j
n n n n
∂h i j
q̇k q̇ j = q̇k q̇ j + q̇k q̇ j ⎠
k=1 j=1
∂qk 2 k=1 j=1 ∂qk k=1 j=1
∂q k
⎛ ⎞
1 ⎝ ∂h i j
n n n n
∂h ik
= q̇k q̇ j + q̇k q̇ j ⎠ , (3.39)
2 k=1 j=1 ∂qk k=1 j=1
∂q j
one gets
n
n
1 ∂h i j ∂h ik ∂h k j
h ci = + − q̇k q̇ j i = 1, . . . , n. (3.40)
k=1 j=1
2 ∂qk ∂q j ∂qi
Let

1 ∂h i j ∂h ik ∂h k j
ck ji = + − = c jki , (3.41)
2 ∂qk ∂q j ∂qi
where ck ji are the Christoffel symbols of the first kind. Based on them, the i jth
element of C(q, q̇) for i, j = 1, . . . , n is given by

n
ci j = ck ji q̇k i, j = 1, . . . , n. (3.42)
k=1
Finally, (3.31)–(3.42) lead to the following equations of motion:
H(q)q̈ + C(q, q̇)q̇ + g(q) = τ . (3.43)
Note that, if desired, friction effects can be added, e.g.
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ , (3.44)

Fig. 3.2 Two degrees of y0

freedom planar robot
m2
q2
/2
m1
q1 x0
/2

where D ∈ Rn×n is a diagonal positive semidefinite matrix accounting for viscous

friction. Part III is devoted to the modeling of some industrial robots, but for the
reader’s comfort a simple example is provided in the following.
Example
Consider the 2-DOF planar robot analyzed in the first example of Sect. 2.2.2.2 and
that it is shown in Fig. 3.2.
For simplicity’s sake both links are considered to be slender rods with the same
length , but different masses m 1 and m 2 , respectively. No friction effects are
included. The first step is to compute the inertia matrix according to (3.27). In fact,
it is not difficult to get from the first example of Sect. 2.4.2.2
⎡ ⎤ ⎡ ⎤
− 2 s1 0 −s1 − 2 s12 − 2 s12
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ c 0⎥
J vc1 =⎢ ⎥ J vc2 =⎢
⎢ c1 +
c
2 12

c ⎥,
2 12 ⎥
(3.45)
⎢ 2 1 ⎥ ⎣ ⎦
⎣ ⎦
0 0 0 0
and
⎡ ⎤ ⎡ ⎤
00 00
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
J wc1 =⎢
⎢0 0⎥⎥ J wc2 =⎢
⎢0 0⎥⎥. (3.46)
⎣ ⎦ ⎣ ⎦
1 0 1 1
Besides it is
⎡ ⎤ ⎡ ⎤
c1 −s1 0 c12 −s12 0
0
Rc1 = ⎣ s1 c1 0 ⎦ 0
Rc2 = ⎣ s12 c12 0 ⎦ . (3.47)
0 0 1 0 0 1
It remains to compute I 1 and I 2 in (3.20). Since slender rods are considered and the
moments and products of inertia are computed with respect to the center of mass of
each link, then it is quite direct to get by using available relationships in basic vector
mechanical books that:
1
Ixxi = 0, Iyyi = Izzi = m i 2 and Ixyi = Ixzi = Iyzi = 0.
12
Therefore one gets
⎡ ⎤
0 0 0
⎢ 1 ⎥
I i = ⎣ 0 12 m i 2 0 ⎦. (3.48)
1
12 m i
2
0 0
By direct computation it is
⎡ ⎤
− 2 s1 0⎡ 2 ⎤
⎡ ⎤⎢ ⎥
− 2 s1 2 c1 0 ⎢ ⎥ l 0
J Tvc1 J vc1 =⎣ ⎦⎢
⎢ c 0⎥ ⎥=⎣
⎢ 4 ⎥
⎦ (3.49)
⎢ 2 1 ⎥
0 0 0 ⎣ ⎦ 0 0
0 0
⎡ ⎤
⎡ ⎤ −s1 − 2 s12 − 2 s12
−s1 − 2l s12 c1 + 2l c12 0 ⎢ ⎢
⎥
⎥
J Tvc2 J vc2 = ⎣ ⎦ ⎢
c1 + 2 c12
c ⎥
⎢ 2 12 ⎥
− 2l s12 l
c
2 12
0 ⎣ ⎦
0 0
⎡ ⎤
5 2 2
2
⎢ 4 + c2 2 c2 + 4 ⎥
2
=⎢⎣
⎥,
⎦ (3.50)
2 c + 2 2
2 2 4 4
and
⎡ ⎤
c −s1 0
001 ⎣ 1 ⎦ 001
J Twc1 0 Rc1 = s1 c1 0 = (3.51)
000 000
0 0 1
⎡ ⎤
c −s12 0
0 0 1 ⎣ 12 001
J Twc2 0 Rc2 = s12 c12 0 ⎦ = . (3.52)
001 001
0 0 1
Thus it is
⎡ ⎤⎡ ⎤
0 0 0 0 0
0 0 1 ⎢ 0 1 m 2 ⎥⎣
J Twc1 0 Rc1 I 1 0 RTc1 J wc1 = 0⎦
0 0 0 ⎣ 12 ⎦ 0
1 0
1
12 m 1
0 0 2 1 0
⎡ 1 ⎤
12 m i 2 0
=⎣ ⎦, (3.53)
0 0
and
⎡ ⎤⎡ ⎤
0 0 0 0 0
0 0 1 ⎢ 0 1 m l2 ⎥⎣
J Twc2 0 Rc2 I 2 0 RTc2 J wc2 = ⎣ 12 2 0 ⎦ 0 0⎦
001 1
0 0 m 2l 2 1 1
⎡ ⎤ 12
1 m 2 1 m 2
2 12 2 ⎥
⎢ 12
=⎣ ⎦. (3.54)
1 m 2 1 m 2
12 2 12 2
Finally the inertia matrix can be gotten as
H(q) = m 1 J Tvc1 J vc1 + m 2 J Tvc2 J vc2

+ J Twc1 0 Rc1 I 1 0 RTc1 J wc1 + J Twc2 0 Rc2 I 2 0 RTc2 J wc2
⎡ ⎤
1 m 2 + 4 m 2 + m l 2 c 1 m 2 + 1 m 2 c
1 2 2 2 2 2 2
⎢3 3 3 2 ⎥
=⎣ ⎦. (3.55)
1 m 2 + 1 m 2 c 1 m 2
3 2 2 2 2 3 2
Once the inertia matrix is known it is straightforward to get the matrix C(q, q̇).
Firstly, the Christoffel symbols are calculated using (3.41):

1 ∂h 11 ∂h 11 ∂h 11
c111 = + − = 0
2 ∂q1 ∂q1 ∂q1

1 ∂h 11 ∂h 12 ∂h 21 1
c211 = + − = − m 2 2 s2
2 ∂q2 ∂q1 ∂q1 2

1 ∂h 12 ∂h 11 ∂h 12 1
c121 = + − = − m 2 2 s2
2 ∂q1 ∂q2 ∂q1 2

1 ∂h 12 ∂h 12 ∂h 22 1
c221 = + − = − m 2 2 s2
2 ∂q2 ∂q2 ∂q1 2

1 ∂h 21 ∂h 21 ∂h 11 1
c112 = + − = m 2 2 s2
2 ∂q1 ∂q1 ∂q2 2

1 ∂h 21 ∂h 22 ∂h 21
c212 = + − = 0
2 ∂q2 ∂q1 ∂q2

1 ∂h 22 ∂h 21 ∂h 12
c122 = + − = 0
2 ∂q1 ∂q2 ∂q2

1 ∂h 22 ∂h 22 ∂h 22
c222 = + − = 0.
2 ∂q2 ∂q2 ∂q2
Then the elements of C(q, q̇) are obtained by employing (3.42)

n
1
c11 = ck11 q̇k = c111 q̇1 + c211 q̇2 = − m 2 2 s2 q̇2
k=1
2

n
1 1
c12 = ck21 q̇k = c121 q̇1 + c221 q̇2 = − m 2 2 s2 q̇1 − m 2 2 s2 q̇2
k=1
2 2

n
1
c21 = ck12 q̇k = c112 q̇1 + c212 q̇2 = m 2 2 s2 q̇1
k=1
2

n
c22 = ck22 q̇k = c122 q̇1 + c222 q̇2 = 0.
k=1
To compute the gravitational vector it is necessary to get the potential energy accord-
ing to (3.29), where
⎡ ⎤ ⎡ ⎤
1 c c1 + 21 c12
⎡ ⎤ ⎢2
1
⎥ ⎢ ⎥
0 ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
ḡ = ⎣ −g0 ⎦ 0
r̄1 = ⎢ 1 s1 ⎥ 0
r̄2 = ⎢ s1 + 1 s12 ⎥ ,
⎢2 ⎥ ⎢ 2 ⎥
0 ⎣ ⎦ ⎣ ⎦
0 0
so that
1 1
P = −m 1 ḡT0 r̄1 − m 2 ḡT0 r̄2 = m 1 g0 ls1 + m 2 g0 ls1 + m 2 g0 ls12 . (3.56)
2 2
By using (3.34) one gets the elements g1 and g2 of g(q) as
1 1
g1 = m 1 g0 c1 + m 2 g0 c1 + m 2 g0 c12 .
2 2
and
1
g2 = m 2 g0 c12 .
2
Summarizing the complete robot model is given by
H(q)q̈ + C(q, q̇)q̇ + g(q) = τ , (3.57)
where
⎡ ⎤
1 m 2 + 4 m 2 + m l 2 c 1 m 2 + 1 m 2 c
1 2 2 2 2 2 2
⎢3 3 3 2 ⎥
H(q) = ⎣ ⎦ (3.58)
1 m 2 + 1 m 2 c 1 m 2
⎡ 3 2 2 2 2 3 2 ⎤
1 1 1
− m 2 s2 q̇2 − 2 m 2 s2 q̇1 − 2 m 2 2 s2 q̇2
2 2
⎢ 2 ⎥
C(q, q̇) = ⎣ ⎦ (3.59)
1 m 2 s q̇ 0
2 2 1
⎡ 2 ⎤
1 m g c + m g c + 1 m g c
2 1 0 1 2 0 1 2 2 0 12
⎢ ⎥
g(q) = ⎣ ⎦. (3.60)
1 m g c
2 2 0 12
As mentioned before, friction effects can be added if desired.
3.3 Inclusion of Environmental Forces
Suppose that the robot end-effector gets in touch with the environment as shown in
Fig. 3.3.
The contact with the object or surface can generate both forces and torques at the
end-effector with respect to the base frame given by
⎡ ⎤
fx
⎢ fy ⎥
⎢ ⎥
⎢ fz ⎥
Fe = ⎢ ⎥
⎢ τx ⎥ . (3.61)
⎢ ⎥
⎣ τy ⎦
τz
For simplicity’s sake throughout the book the vector Fe ∈ R6 will be referred to
as external forces only and it does not need to be of dimension 6 if n < 6 or if
the dimension of motion constraints, say m, is smaller than n. For that case n −
m elements of Fe will be zero. To find out how these forces are reflected at the
generalized joints, the principle of virtual work can be employed. If δq represents
the virtual joint displacement caused by Fe , then according to (2.183) the virtual
end-effector displacement must satisfy
Fig. 3.3 Robot manipulator in contact with its environment

3.3 Inclusion of Environmental Forces 87
δx = J(q)δq. (3.62)
Since the force opposes to the movement of the end-effector caused by the input
torque, then the virtual work can be computed as

δW = FTe δx − τ Te δq = FTe J(q) − τ Te δq = 0, (3.63)
from which the following relationship can be obtained
τ e = J T (q)Fe . (3.64)
This relationship can be directly inserted in (3.44) to get
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ − J T (q)Fe . (3.65)
An issue of particular interest is how to obtain, compute or represent Fe . Assume for

example that the surface shown in Fig. 3.3 can be represented by a set of holonomic
constraints given by
ϕ(q) = 0, (3.66)
with ϕ(q) ∈ Rm and m represents the number of constraints as mentioned before. If

ϕ j (q) is the jth element of ϕ(q) for j = 1, . . . , m, then its derivative satisfies
d n
d
ϕ̇ j (q) = ϕ j (q) = a ji qi = 0, (3.67)
dt i=1
dt
for i = 1, . . . , n and where
∂ϕ j (q)
a ji = . (3.68)
∂qi
In terms of virtual displacements one must therefore have

n
a ji δqi = 0, (3.69)
i=1
while once again the effect on the system dynamics is that of forces which do not
do work. Let’s denote the generalized torque associated to δqi as τ̄i (might just be a
force as well), so that in view of the Principle of Virtual Work it must hold

n
τ̄i δqi = 0 (3.70)
i=1
if the virtual displacements are consistent with the constraints. Now, suppose that
each equation (3.69) is multiplied by a so called Lagrange multiplier λ j , i.e.

n
λj a ji δqi = 0 j = 1, . . . , m. (3.71)
i=1
Quite obviously it is

m
n
n
m
λj a ji δqi = λ j a ji δqi = 0, (3.72)
j=1 i=1 i=1 j=1
where it has been taken advantage of the fact that the sums are interchangeable. Also,
it is possible to add this relationship to (3.70) in order to get
⎛ ⎞

n
n
m
n
m
τ̄i δqi + λ j a ji δqi = ⎝τ̄i + λ j a ji ⎠ δqi = 0. (3.73)
i=1 i=1 j=1 i=1 j=1
At this point the multipliers are completely arbitrary while on the contrary the virtual
displacements δqi must comply with (3.69). However, if the multipliers are chosen
to satisfy

m
τ̄i = − λ j a ji i = 1, . . . , n, (3.74)
j=1
then (3.73) holds for any set δqi . Therefore, since τ̄i are generalized torques acting
at the joints, they can be included directly in (3.44) as
⎡ ⎤⎡ ⎤
a11 · · · am1 λ1
⎢ .. .. ⎥ ⎢ .. ⎥ .
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ − ⎣ . . ⎦⎣ . ⎦ (3.75)
a1n · · · amn λm
By defining the vector of Lagrange multipliers λ ∈ Rm as

⎡ ⎤
λ1
⎢ ⎥
λ = ⎣ ... ⎦ , (3.76)
λm
then it is possible to write (3.75) as
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ − J Tϕ (q)λ, (3.77)

3.3 Inclusion of Environmental Forces 89
where
J ϕ (q) = ∇ϕ(q). (3.78)
∇ϕ(q) ∈ Rm×n denotes the gradient of the surface vector ϕ(q), which maps a vector
onto the normal plane at the tangent plane that arises at the contact point described
by (3.66). J ϕ (q) is assumed to be full rank. This must indeed be the case because J ϕ (q)
provides the directions where it is possible to apply forces and having Rank(J ϕ (q)) <
m would mean that the constraints are redundant because for that case at least two
rows of J ϕ (q) would be linearly dependent. Also note that usually it is possible to
normalize the constraints to get the rows of J ϕ (q) with unitary magnitude, so that
each element of λ physically represents the force magnitude applied at the contact
point in the direction given by the corresponding row of J ϕ (q).
It is worthy to point out that λ can be computed in the following way. From (3.66)
it is
ϕ̇(q) = J ϕ (q)q̇ = 0, (3.79)
and
ϕ̈(q) = J ϕ (q)q̈ + J̇ ϕ (q)q̇ = 0. (3.80)
But from (3.77) one has from

q̈ = H −1 (q) τ − τ̄ − J Tϕ (q)λ , (3.81)
with τ̄ = C(q, q̇)q̇ + Dq̇ + g(q). Substituting in (3.80) it is

J ϕ (q)H −1 (q) τ − τ̄ − J Tϕ (q)λ + J̇ ϕ (q)q̇ = 0. (3.82)
Finally, the Lagrange multipliers are given by

−1
λ = J ϕ (q)H −1 (q)J Tϕ (q) J ϕ (q)H −1 (q){τ − τ̄ } + J̇ ϕ (q)q̇ . (3.83)
Writing the constraints in joint space coordinates allows to develop in a direct

way the computation of Lagrange multipliers, but in fact it is more straightforward
to write them in work space coordinates as
ϕ(x) = 0, (3.84)
so that one also has

∂ϕ(x)
J ϕx = (3.85)
∂x
meaning that
J ϕ (q) = J ϕx J(q). (3.86)
Therefore, model (3.77) can be rewritten as
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ − J T (q)J Tϕx λ. (3.87)
Just by comparing with (3.65) it can be concluded that
Fe = J Tϕx λ. (3.88)
3.4 Model Properties
Some useful properties regarding model (3.44) are presented in this section
3.4.1 Vectors and Matrices Properties
One of the most important and better known properties of model (3.44) is the fol-
lowing.
Property 3.1 Positive definiteness of H(q). The generalized inertia matrix H(q) is
symmetric positive definite.
Proof From (3.27) it is evident that H(q) is symmetric. On the other hand, the kinetic
energy of any mechanical system can be zero only if it is at rest, i.e. if q̇ ≡ 0, so that
from (3.26) the positive definiteness is granted.
Another property that can be exploited for control and observer design is the
following.
Property 3.2 By using the Christoffel symbols of the first kind to compute C(q, q̇),
the matrix N(q, q̇) = Ḣ(q) − 2C(q, q̇) ∈ Rn×n is skew symmetric.
Proof Each element of Ḣ(q) satisfies

n
∂h i j
ḣ i j = q̇k i, j = 1, . . . , n. (3.89)
k=1
∂qk
By taking into account (3.41) and (3.42) the elements of N(q, q̇) can be computed
as
3.4 Model Properties 91
n

∂h i j ∂h i j ∂h ik ∂h k j
n i j = ḣ i j − 2ci j = − + − q̇k
k=1
∂qk ∂qk ∂q j ∂qi

∂h k j
n
∂h ik
= − q̇k .
k=1
∂q i ∂q j
Since
n

∂h ki ∂h jk
n ji = − q̇k ,
k=1
∂q j ∂qi
and h i j = h ji then it is n i j = −n ji , which concludes the proof.
It should be noted that Property 3.2 is based on the employment of the Christoffel
symbols to compute C(q, q̇). Note that in general there may exist many matrices
which satisfy (3.37) and for which the previous property does not hold. However,
the following relationship is always satisfied:
q̇T (Ḣ(q) − 2C(q, q̇))q̇ = 0. (3.90)
To show this Hamilton’s equations of motion can be used. The first step consists in
writing (3.31) and (3.44) as
d ∂L ∂L
− = ψ = H(q)q̈ + C(q, q̇)q̇ + g(q), (3.91)
dt ∂ q̇ ∂q
where
ψ = τ − Dq̇. (3.92)
The Hamiltonian or Hamiltonian function is given by
H = pT q̇ − L, (3.93)
where the generalized momentum p is
∂L
p= . (3.94)
∂ q̇
From (3.30) it is
∂L
= H(q)q̇. (3.95)
∂ q̇
Therefore, from (3.93) and (3.94), the Hamiltonian is given by
1 1
H = q̇T H(q)q̇ − q̇T H(q)q̇ + P = q̇T H(q)q̇ + P = K + P. (3.96)
2 2
On the other hand, Hamilton’s canonical equations of motion can be expressed
as
∂H
q̇i = (3.97)
∂ pi
∂H
ṗi = − + ψi , i = 1, . . . , n. (3.98)
∂qi
From (3.97) and (3.98) the time derivative of H is
dH ∂Hn ∂Hn
= q̇i + ṗi
dt i=1
∂qi i=1
∂ pi
n
∂H ∂H ∂H ∂H ∂H
n n
= − + ψi
i=1
∂qi ∂ pi i=1
∂ pi ∂qi i=1
∂ pi
= q̇T ψ. (3.99)
But the derivative can also be computed from (3.91) and (3.96):
dH 1 ∂P
= q̇T H(q)q̈ + q̇T Ḣ(q)q̇ + q̇T
dt 2 ∂q
1
= q̇T (ψ − C(q, q̇) − g(q)) + q̇T Ḣ(q)q̇ + q̇T g(q)
2
1
= q̇T ψ + q̇T Ḣ(q) − 2C(q, q̇) q̇. (3.100)
2
By comparing (3.99) and (3.100) one gets (3.90). In conclusion, Property 3.2 is a
particular case of (3.90).
Property 3.3 By using the Christoffel symbols of the first kind to compute C(q, q̇),
the derivative of the inertia matrix satisfies Ḣ(q) = C(q, q̇) + CT (q, q̇).
Proof It can be shown by direct computation as done for Property 3.2.
Property 3.4 The vector of Coriolis and centrifugal torques hc (q, q̇) satisfies:
hc (q, x, y) = C(q, x)y = C(q, y)x = hc (q, y, x) ∀x, y ∈ Rn . (3.101)

Proof From (3.40) and (3.41), the i-element of hc (q, x, y) can be expressed as
⎛ ⎞

n
n
h ci (q, x, y) = ⎝ ck ji xk ⎠ y j
k=1 j=1
n

n
= c jki y j xk = h ci (q, y, x). (3.102)
j=1 k=1

3.4.2 Norm Related Properties
Finding norm bounds for the different matrices and vectors of model (3.44) plays an
important role in proving closed loop stability for control-observer design. In fact, for
any physical system both q and q̇ must be bounded. Therefore, for simplicity’s sake
this section makes the assumption that the manipulator owns only revolute joints,
although the properties are also valid for robots with prismatic joints.
Property 3.5 The inertia matrix H(q) satisfies
λmin (H(q))y2 ≤ yT H(q)y ≤ λmax (H(q))y2 ∀y ∈ Rn . (3.103)
Proof Since H(q) is positive definite, each vector y in Rn can be expressed in terms
of an orthonormal basis (y1 , . . . , yn ) as y = c1 y1 + · · · + cn yn which satisfies:
yT H(q)y = c12 λ1 (H(q)) + · · · + cn2 λn (H(q)) (3.104)
and
yT y = y2 = c12 + · · · + cn2 , (3.105)
from where (3.103) holds.
Property 3.6 The inverse H−1 (q) exists and satisfies

−1
λ−1 2 T −1
max (H(q))y ≤ y H (q)y ≤ λmin (H(q))y
2
∀y ∈ Rn . (3.106)
Proof This property follows directly from Property 3.5.
Property 3.7 The inertia matrix satisfies 0 < λh ≤ H(q) ≤ λH < ∞.
Proof Since revolute joints are considered it is possible to conclude from (3.103)
that
λh = minn λmin (H(q)) λH = maxn λmax (H(q)). (3.107)

q∈R q∈R
Property 3.8 H−1 (q) satisfies 0 < σh ≤ H−1 (q) ≤ σH < ∞.
Proof The proof is the same as for Property 3.7 with
σh = minn λ−1
max (H(q)) σH = maxn λ−1
min (H(q)). (3.108)
q∈R q∈R
Property 3.9 The matrix C(q, y) satisfies C(q, y) ≤ kc y, kc > 0.
Proof From (3.42) it can be seen that the matrix C(q, y) can be written as
1
n
C(q, y) = Bk (q)yk , (3.109)
2 k=1
where the i jth element of Bk (q) ∈ Rn×n is defined as
∂h i j ∂h ik ∂h k j
+ − i, j = 1, . . . , n.
∂qk ∂q j ∂qi
Computing the norm of C(q, y) leads to

n
1
1 1
n n

C(q, y) = Bk (q)yk ≤ Bk (q)yk ≤ Bk (q) y.
2 k=1 2
k=1
2 k=1
Since the matrices Bk (q) are functions of bounded variables, they are bounded so
that Property 3.9 holds by defining:
1
n
kc = maxn Bk (q). (3.110)
2 q∈R k=1
Property 3.10 The matrix D is diagonal positive semidefinite and satisfies:
λmin (D)y2 ≤ yT Dy ≤ λmax (D)y2 ∀y ∈ Rn . (3.111)
Proof D is positive semidefinite because it is defined based on the Rayleigh dissi-

pation function:
1 T
D= q̇ Dq̇. (3.112)
2

Assuming D to be diagonal is actually a particular but important case of this
function. Equation (3.111) is valid because D is positive semidefinite.
Property 3.11 The vector of gravitacional torques satisfies g(q) ≤ kg , kg > 0.
Proof The property follows directly from (3.33) by recalling that revolute joints are
being considered.
Property 3.12 The vector of gravitational torques g(q) satisfies

∂g(q)

∂q ≤ αg . (3.113)
Proof Since revolute joints are being considered, the generalized coordinates q
appear in g(q) only as argument of (bounded) sine and cosine functions, and that
∂g(q)
must be the case for ∂q , from where the property follows.
Property 3.13 The vector of gravitational torques g(q) satisfies
g(q1 ) − g(q2 ) ≤ αg q1 − q2 ∀q1 , q2 ∈ Rn . (3.114)
Proof Property 3.13 follows from Property 3.12 by applying the Mean Value The-
orem. Since g(q) is a continuous differentiable function and its partial derivatives
with respect to q are bounded, then the following relationship must be satisfied for
any two vectors q1 and q2 :
∂g(q)
g(q1 ) − g(q2 ) = (q1 − q2 ), (3.115)
∂q q12
where q12 is a point between q1 and q2 . By taking norms one gets:

∂g(q) ∂g(q)

g(q1 ) − g(q2 ) = (q1 − q2 ) ≤ q1 − q2 , (3.116)
∂q q12 ∂q q12
so that (3.114) holds by choosing

∂g(q)

αg = maxn (3.117)
∀q∈R ∂q

3.4.3 Whole Model Related Properties
This section provides two properties of model (3.44) as a whole. The first one is the
based of adaptive control for robot manipulators, while the second one exploits a key
physical characteristic.
Property 3.14 With a proper choice of parameters, model (3.44) can be rewritten as
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ = Y(q, q̇, q̈)ϕ, (3.118)
where Y(q, q̇, q̈) ∈ Rn× p is known as the regressor, and ϕ ∈ R p is a vector of param-
eters.
The proof of Property 3.14 is omitted because it is based on the Newton-Euler
equations of motion. Instead, consider the following example.
Example
Consider the robot model given in (3.57)–(3.60) written as

! "
1 m l 2 + 4 m l 2 + m l 2c 1 m l 2 + 1 m l 2c
3 1 3 2 2 2 3 2 2 2 2 q̈1
1 m l 2 + 1 m l 2c 1 m l2 +
q̈ 2
3 2 2 2 2 3 2
⎡ ⎤
− 21 m 2 l 2 s2 q̇2 − 21 m 2 l 2 s2 q̇1 − 21 m 2 l 2 s2 q̇2
⎢ ⎥ q̇1
⎣ ⎦ q̇ +
1 m l 2 s q̇ 2
2 2 2 1 0
⎡ ⎤
1 m g lc + m g lc + 1 m g lc
1 0 1 2 0 1 2 0 12
⎢2 2 ⎥ τ1
⎣ ⎦= τ
1 2
2 m 2 g0 lc12
First of all it is necessary to find a vector ϕ. In general there are many possible
choices of parameters, but the less the better. For instance it can be chosen
⎡ ⎤ ⎡1 ⎤
ϕ1 3 m 1l 2 + 43 m 2 l 2
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎢
⎥
⎥
⎢ ϕ2 ⎥ ⎢ m l 2
⎥
⎢ ⎥ ⎢ 2
⎥
ϕ=⎢ ⎢ ⎥ ⎢
⎥=⎢ ⎥,
⎢ ϕ3 ⎥ ⎢ ⎥
⎢ ⎥ ⎢
1m g l ⎥
⎣ ⎦ ⎣ 2 1 0 ⎥
⎦
ϕ4 m 2 g0 l
so that the robot model can be rewritten as

⎡ ⎤ ⎡ ⎤
ϕ1 + ϕ2 c2 1ϕ + 1ϕ c − 1 ϕ2 s2 q̇2 − 21 ϕ2 s2 q̇1 − 21 ϕ2 s2 q̇2
⎢ 3 2 2 2 2 ⎥ q̈1 ⎢ 2 ⎥ q̇1
⎣ ⎦ q̈ + ⎣ ⎦ q̇
1ϕ + 1ϕ c 1ϕ 2 1 ϕ s q̇ 2
3 2 2 2 2 3 2 2 2 2 1 0
! "
ϕ3 c1 + ϕ4 c1 + 21 ϕ4 c12 τ
= 1 .
1
ϕ 4 c 12 τ2
2
Finally, the regressor can be computed as

⎡ # $ # $ ⎤
q̈1 c2 q̈1 + 31 + 21 c2 q̈2 − 21 s2 q̇2 q̇1 − 21 s2 q̇1 + 21 s2 q̇2 q̇2 c1 c1 + 21 c12
⎢ ⎥
⎢ ⎥ ϕ = τ.
⎣ # $ ⎦
0 1 + 1 c q̈ + 1 q̈ + 1 s q̇ q̇ 0 1c
3 2 2 1 3 2 2 2 1 1 2 12

Y(q̈,q̇,q)
Property 3.15 The dynamic equation of motion of a robot manipulator given

by (3.44) defines a passive mapping ψ → q̇, i.e.
T
< q̇, ψ >T = q̇T ψdt ≥ −β, (3.119)
0
for some β > 0 and ∀ T > 0, with ψ = τ − Dq̇ ∈ Rn .

Proof Recall that the derivative of the system’s Hamiltonian is given by (3.99)
T T
dH
= q̇T ψ ⇒ < q̇, ψ >T = q̇ ψdt =
T
dH = H(T ) − H(0) ≥ −H(0).
dt
0 0
Since H(T ) is not negative for all T , Property 3.15 follows with β = H(0). Note
that alternatively it can be done
T
< q̇, ψ >T = q̇T ψdt
0
T # $
= q̇T H(q)q̈ + q̇T C(q, q̇)q̇ + q̇T g(q) dt
0
T
1 d T 1
= q̇ H(q)q̇ − q̇T Ḣ(q) − 2C(q, q̇) q̇ + q̇T g(q) dt
2 dt 2
0
T
d 1 T
= q̇ H(q)q̇ + gT (q)q̇ dt.
dt 2
0
Note that to arrive to the last relationship (3.90) was used, showing that (3.90) is
closely related to the passivity property. On the other hand, from (3.34) it is known
that2
∂ P
gT (q) = .
∂q
Therefore it is
∂ P
gT (q)q̇ = q̇ = Ṗ.
∂q
By considering (3.26), it can be written
T
d
< q̇, ψ >T = (K + P) dt
dt
0
= (K + P)|0T
= H|0T
= H(T ) − H(0)
≥ −H(0).

3.4.4 Holonomic Constraints Properties
When the robot end-effector is in contact with its environment and this can be
described by the holonomic constraint (3.84), then for model (3.87) the following
property holds.
Property 3.16 The vector ẋ can be written as
ẋ = Qx (x)ẋ + Px (x)ẋ = Qx (x)ẋ, (3.120)

T −1
where Qx (x) = (In×n − Px (x)), Px (x) = J+ +
ϕx Jϕx , and Jϕx = Jϕx Jϕx Jϕx
T
∈ Rn×m
stands for the Penrose’s pseudoinverse and Qx ∈ Rn×n satisfies rank (Qx ) = n −
m. These two matrices are orthogonal, i.e. Qx Px = O (and in fact Qx JTϕx = O and
2Recall that in (3.34) some abuse of notation was made by omitting the transpose which is included
here for convenience.
Jϕx Qx = O). Note that the last equality in (3.120) is due to the fact that ϕ̇(x) =
Jϕx ẋ = 0 in view of constraint (3.84). Finally, it holds
Qx (x)Qx (x) = Qx (x) and Px (x)Px (x) = Px (x). (3.121)
3.5 Inclusion of DC Motors in the Robot Dynamic Model
The generalized input torque τ given in models (3.44) or (3.65) is given without
considering at all the physical actuators. In this section, the inclusion of DC motors
dynamics will be considered because this is the kind of actuators provided in all the
robots employed for experiments in this book.
Figure 3.4 shows how a DC motor can be connected to the ith joint of a robot
manipulator, for i = 1, . . . , n. The physical quantities involved in the ith motor
dynamics are the following:
K ai motor-torque constant (Nm/A)
Rai armature resistence ()
L ai armature inductance (H)
K bi back emf constant (Vs/rad)
τmi torque at the motor axis (Nm)
i ai armature current (A)
ebi back emf (V)
qmi angular position of the motor axis (rad)
qi angular position of the mechanical load axis (rad)
ri gears reduction ratio (in general ri >> 1)
Vi armature voltage (V)
The mechanical load for the present study is the ith link of the robot. The different
variables and constants can be related through the following relationships
Fig. 3.4 Schematic Gearbox

connection of a DC motor
used as actuator for a robot
joint
JL Link
qi
Jm fLi
qmi
Motor 1 : ri
fmi
τmi = K ai i ai (3.122)
di ai
Vi = Rai i ai + L ai + ebi (3.123)
dt
ebi = K bi q̇mi (3.124)
qmi = ri qi . (3.125)
Note that the armature inductance L ai is usually assumed to be negligible (L ai ≈ 0),

so that (3.123) can be simplified. On the one hand, the motor’s equation of motion
can be written as τi
Jmi q̈mi = τmi − f mi (q̇mi ) − , (3.126)
ri
where τi is the applied net torque after the gear reduction on the mechanical load
axis (i.e. the generalized input torque in the robot modeling), Jmi is the rotor inertia
and f mi (q̇mi ) represents friction effects.
By recalling that L ai ≈ 0, i ai can be substituted from (3.122) into (3.123) and the
resulting τmi can be substituted from (3.123) into (3.126) to get
K ai τi K ai
Jmi q̈mi + f m (q̇mi ) + ebi + = Vi . (3.127)
Rai ri Rai
Using (3.124) and (3.125) leads to
K ai τi K ai
ri Jmi q̈i + f mi (ri q̇i ) + K bi ri q̇i + = Vi , (3.128)
Rai ri Rai
or
1 K ai K bi τi K ai
Jmi q̈i + f mi (ri q̇i ) + q̇i + 2 = Vi . (3.129)
ri Rai ri ri Rai
Assume now that only joint viscous friction effects are present so that it is
f mi (q̇mi ) = f mi q̇mi , (3.130)
where f mi is the joint viscous constant that can eventually be neglected. With this
simplification (3.129) becomes
% &
K ai K bi τi K ai
Jmi q̈i + f mi + q̇i + 2
= Vi , (3.131)
Rai ri r i Rai
for i = 1, . . . , n. In compact matrix form one has
Dj q̈ + Df q̇ + Dn τ = DK V, (3.132)
with
References 101
' mi } #
Dj = diag{J $(
Df = diag f mi + K aiRK bi
% & ai
Dn = diag 2 1
'# r$ i (
DK = K ai 1
Rai ri
Therefore, the generalized input torque vector is given by

τ = D−1
n DK V − Dj q̈ − Df q̇ , (3.133)
which can be substituted in the robot model. For example, consider (3.65) to obtain

H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = D−1
n DK V − Dj q̈ − Df q̇ − J T (q)Fe . (3.134)
It is possible to rewrite (3.134) as

H(q) + D−1 −1
n Dj q̈ + C(q, q̇)q̇ + D + Dn Df q̇ + g(q) =
D−1
n DK V − J (q)F e .
T
(3.135)
It is not difficult to show that all the properties given in Sect. 3.4 still hold, although
care must be taken since for Property 3.14 the generalized input has become D−1 n DK V
and, in general, Dn and DK must be assumed to be known since different constant
values contained there do not form part of the parameter vector ϕ.
References
An CH, Atkeson CG, Hollerbach JM (1985) Estimation of inertial parameters of rigid body links of
manipulators. In: Proceedings of 24th IEEE conference on decision and control, Ft. Lauderdale,
FL, USA, Dec 1985, pp 990–995
Arteaga-Pérez MA (1998) On the properties of a dynamic model of flexible robot manipulators.
ASME J Dyn Syst Meas Control 120:8–14
Atkeson CG, An CH, Hollerbach JM (1985) Rigid body load identification for manipulators. In:
Proceedings of 24th IEEE conference on decision and control, Ft. Lauderdale, FL, USA, Dec
1985, pp 996–1002
Beer FP, Johnston ER Jr, Mazurek DF, Cornwell PJ (2013) Vector mechanics for engineers: statics
and dynamics, 10th edn. McGraw-Hill, USA
Feng W, Postlethwaite I (1993) A simple robust control scheme for robot manipulators with only
joint position measurements. Int J Robot Res 12(5):490–496
Hill, USA
Greenwood DT (1997) Classical dynamics. Dover Publications, INC, Mineola, New York
Kelly R, Santibáñez V, Loria A (2005) Control of robot manipulators in joint space. Springer,
London
Meirovitch L (1967) Analytical methods in vibrations. The Macmillan Company, New York, USA

Press, Boca Raton, FL, USA
Ortega R, Spong MW (1989) Adaptive motion control of rigid robots: a tutorial. Automatica
25(6):877–888
Strang G (2006) Linear algebra and its applications, 4th edn. Thompson Brooks, USA
Wellstead PE (2005) Introduction to physical system modelling. Control systems principles. https://
www.control-systems-principles.co.uk
Chapter 4
Mathematical Background
Abstract Robot manipulators are highly nonlinear dynamic systems. When a

desired task has to be accomplished, there are many issues to be solved to get the
best possible performance. First of all the task has to be planned and expressed in
such a way that it can be executed by the robot. Perhaps the most common practice
consists in defining particular desired trajectories or references for each robot joint
and then implement a control algorithm to follow these trajectories. This may well
imply the computation of inverse kinematics online, but assuming that this is done,
then the most relevant challenge becomes the design of a control scheme. Also the
most common approach is to define an error between desired and actual trajectories,
so that if it vanishes then the task is accomplished. There are many analytical tools
for control design, each of them with pros and cons, e.g. passivity or Lyapunov the-
ory. In this section, we focus on the latter and provide the reader with the most basic
definitions and theorems that will be employed in the rest of the book in the solution
of many different problems.
4.1 Basic Definitions and Lemmas
The goal of this chapter is to provide the basic theory necessary to develop all the
control and observer schemes in the next chapters. Although this makes the book self
contained, by no means it can be considered as a complete background of analytical
tools since most of the results are given without proof and the interested reader should
look into the references for a deeper insight. However, sketches of the proofs of the
main theorems are provided in order to better understand the control-observer design
proposed in this work. This section lists some very well known basic definitions,
concepts and important remarks.
Definition 4.1 Consider the n-dimensional vector x=[x1 · · · xn ]T , where the scalars
x1 , . . . , xn belong to R, then the set of all vectors define the n-dimensional Euclidean
space denoted by Rn .
Definition 4.2 A set of vectors xi (t) ∈ Rn , i = 1, . . . , p are said to be linearly

independent on the interval [t1 , t2 ] if and only if
https://doi.org/10.1007/978-3-030-85980-0_4
104 4 Mathematical Background

p
xi (t)αi = 0, t ∈ [t1 , t2 ], (4.1)
i=1
holds only for the trivial solution αi = 0, ∀ i = 1, . . . , p. Otherwise, the vectors are
said to be linearly dependent on the interval [t1 , t2 ].
According to this definition, the vectors xi ∈ Rn are linearly independent if it is

not possible to express any of them as a combination of the others. Also, if they are
constant the interval [t1 , t2 ] can be dropped in Definition 4.2. This concept can be
expressed in a more compact form by using a matrix X ∈ Rn× p

X = x1 , . . . , x p (4.2)
and a vector α ∈ R p . According to Definition 4.2, the columns of X(t) are linearly
independent on the interval [t1 , t2 ] if and only if
X(t)α = 0 ⇒ α = 0, t ∈ [t1 , t2 ]. (4.3)
For a constant matrix X their columns are linearly independent if and only if
Xα = 0 ⇒ α = 0. (4.4)
Theorem 4.1 The columns of a matrix X ∈ Rn× p are linearly independent on the
interval t ∈ [t1 , t2 ] if and only if the matrix
t2
1
B= XT (t)X(t)dt (4.5)
t2 − t1
t1
is not singular. Additionally, the rows of X ∈ Rn× p are linearly independent on the
interval t ∈ [t1 , t2 ] if and only if the matrix
t2
1
B= X(t)XT (t)dt (4.6)
t2 − t1
t1
is not singular.
Definition 4.3 The norm x of a vector is a real-value function satisfying:

• x ≥ 0 ∀x ∈ Rn , with x = 0 if and only if x = 0
• Triangle inequality: x + y ≤ x + y ∀x, y ∈ Rn
• αx = |α|x ∀α ∈ R, ∀x ∈ Rn
In this book, x stands for the standard Euclidean norm if x is a vector, i.e.
4.1 Basic Definitions and Lemmas 105
1 1
x = x12 + · · · + xn2 2 = xT x 2 , (4.7)
while ||A|| denotes the corresponding induced 2-norm for a matrix A

1
A = max λi AT A 2 , (4.8)
i
where λi denotes the i-th eigenvalue for i = 1, . . . , n. For any function f (t) :
[0, ∞) → Rn , the L∞ -norm is defined as
f (t)∞ = sup f (t). (4.9)

t≥0
The L2 -norm is defined as

∞
1
2
f (t)2 = f (t) dt
2
. (4.10)
0
Definition 4.4 Let f (t, x) be piecewise continuous in t. f (t, x) is locally Lipschitz

in x on [t1 , t2 ] × D ⊂ R × Rn if each point x ∈ D has a neighborhood D0 such that
f (t, x1 ) − f (t, x2 ) ≤ L 0 x1 − x 2 (4.11)
holds on [t1 , t2 ] × D0 with some Lipschitz constant L 0 . f (t, x) is locally Lipschitz

in x on [t0 , ∞) × D if it is locally Lipschitz in x on [t1 , t2 ] × D for every compact
interval [t1 , t2 ] ⊂ [t0 , ∞). f (t, x) is Lipschitz in x on [t0 , t1 ] × D if (4.11) is satisfied
with the same Lipschitz constant L for all points x ∈ D and for all t ∈ [t1 , t2 ]. The
term globally Lipschitz is used in case D = Rn .
The Lipschitz condition (4.11) is related to the existence and uniqueness of the
solution of differential equations, but for the systems under study in this book such
solutions always exist. However, this condition is also necessary to guarantee stability
in the sense of Lyapunov.
Definition 4.5 It is said that a continuous function α : [0, a) → [0, ∞) belongs to

1. The class K if it is strictly increasing and α(0) = 0
2. The class K∞ if a = ∞ and α(r ) = ∞ if r → ∞
Definition 4.6 It is said that a continuous function β : [0, a) × [0, ∞) → [0, ∞)

belongs to the class KL, if for each fixed q, β( p, q) belongs to the class K with
respect to p and, for each fixed p, β( p, q) is decreasing with respect to q and
β( p, q) → 0 as q → ∞.
Remark 4.1 It can be shown that if α1 (·) is a class K function on [0, a), then the
inverse α1−1 (·) exists, it is defined on [0, α1 (a)) and belongs to the class K as well.
Furthermore if α2 (·) is another class K function on [0, a), then the composition α1 ◦
α2 belongs to the class K as well. The very same conclusions can be drawn for class
K∞ functions. Finally, if β(·, ·) is a class KL function σ ( p, q) = α1 (β(α2 ( p), q))
belongs to the class KL as well.
Definition 4.7 A scalar continuous function V (t, x) is said to be

1. Locally positive definite in the domain D ⊂ Rn which contains the origin if
V (t, 0) = 0 (4.12)
holds for all t ≥ t0 , and there exits a class K function α1 (x) such that
α1 (x) ≤ V (t, x) (4.13)
holds for all x ∈ D and for all t ≥ t0 .

2. Decrescent if there exits a class K function α2 (x) such that
V (t, x) ≤ α2 (x) (4.14)
holds for all x ∈ D and for all t ≥ t0 .

3. Positive definite if (4.12)–(4.13) hold for D = Rn and α1 (x) belongs to the
class K∞ .
4. Negative definite in the domain D if −V (t, x) is positive definite.
5. Positive semi-definite in the domain D if it is non-negative there.
6. Negative semi-definite in the domain D if it is non-positive there.
7. Indefinite in the domain D if it does not have definite sign.

Remark 4.2 A much simpler definition of a positive definite function implies chang-
ing the class K function α1 (x) by a function W1 (x) satisfying i) W1 (0) = 0, ii)
W1 (x) > 0 for x = 0. However, it is always possible to find a class K function α1 (x)
so that α1 (x) ≤ W1 (x) holds. A similar W2 (x) can be employed in (4.14), but for
that case a class K function can always be found so that W2 (x) ≤ α2 (x) holds.
Therefore, conditions (4.13) and (4.14) could be redefined in terms of W1 (x) and
W2 (x), respectively. Finally, if V (t, x) is positive definite as given in Definition 4.7.3
then it is also said to be radially unbounded. In terms of W1 (x) one would have
W1 (x) → ∞ as x → ∞.
Lemma 4.1 (Comparison lemma) Let
ẋ = f (t, x) x(t0 ) = x0 , (4.15)
where f (t, x) is continuous in t and locally Lipschitz in x, for all t ≥ t0 and all
x ∈ D ⊂ R. Assume that [t0 , t1 ) is the maximal interval of existence of the solution
x(t) such that x(t) ∈ D. Let y(t) be a continuous function whose upper right-hand
derivative D + y(t) satisfies
4.1 Basic Definitions and Lemmas 107
D + y(t) ≤ f (t, y(t)) y(t0 ) ≤ x0 (4.16)
with y(t) ∈ D for all t ∈ [t0 , t1 ). Then, y(t) ≤ x(t) for all t ∈ [t0 , t1 ).
Lemma 4.2 Consider the scalar differential equation
ẋ(t) = −α1 (x(t)) x(t0 ) = x0 , (4.17)
where α1 (x) is a locally Lipschitz class K function defined on [0, a). For any x0 ∈
[0, a), the unique solution x(t) of (4.17) satisfies
x(t) = β(x0 , t − t0 ) ∀ t ≥ t0 , (4.18)
where β(·, ·) is a class KL function [0, a) × [0, ∞).
Lemma 4.3 For any vector signals x, y, any variable time delay 0 ≤ T (t) ≤ T <
∞ and any constant α > 0, it holds
t σ 2
α T
− xT (σ ) y(θ ) dθ dσ ≤ x22 + y22 . (4.19)
2 2α
0 σ −T (σ )
Lemma 4.4 For any x(t) : R ≥ t0 → Rn , if x(t) ∈ L∞ ∩ L2 and ẋ(t) ∈ L∞ , then
lim x(t) = 0. (4.20)

t→∞
4.2 Stability in the Sense of Lyapunov
4.2.1 Definition
Consider the vector differential equation
ẋ(t) = f (t, x(t)) t ≥ t0 , (4.21)
where x(t) ∈ Rn is the state and f : R+ × Rn → Rn . It is understood that a solution

of (4.21) in an interval [t0 , t1 ] is a continuous function x : [t0 , t1 ] → Rn such that
ẋ(t) is defined and ẋ(t) = f (t, x(t)) for all t ∈ [t0 , t1 ]. We assume that there exist a
unique solution over [t0 , ∞) for each initial condition x0 = x(t0 ) and that it depends
continuously on x0 , as for f satisfying a global Lipschitz condition. Figure 4.1 shows
a phase-plane plot of the solution of a vector differential equation for n = 2, i.e.
Fig. 4.1 Possible trajectory x2

for the solution of a vector
differential equation
x(t0 )
x1
x(t1 )
x(t) ∈ R2 . Throughout the rest of the chapter all phase-plane are depicted for R2 as
representative examples, but the theory is completely valid for Rn .
A particular x0 for which
f (t, x0 ) ≡ 0, (4.22)
is called an equilibrium point. Both for simplicity’s sake and because it is conceptually
the most important case for the development of the rest of the book, we assume
that 0 is an equilibrium point of (4.21). Note that this does not imply any lost of
generality and that there may well exist more than one single equilibrium point for a
particular system. The name of equilibrium point is well chosen because if x(t0 ) = 0
then x(t) ≡ 0 for all t ≥ t0 . But the natural question is, what happens if x(t0 ) = 0?
Consider the following definition.
Definition 4.8 The equilibrium point 0 at time t0 of (4.21) is said to be

1. Stable at time t0 if, for each > 0, there exists a δ(t0 , ) > 0 such that
x(t0 ) < δ(t0 , ) ⇒ x(t) < , ∀t ≥ t0 (4.23)
2. Uniformly stable over [t0 , ∞) if, for each > 0, there exists a δ() > 0 such that
x(t0 ) < δ() ⇒ x(t) < , ∀ t ≥ t0 (4.24)
3. Asymptotically stable at time t0 if

1. It is stable at time t0 , and
2. There exists a δ1 (t0 ) > 0 such that
4.2 Stability in the Sense of Lyapunov 109
x(t0 ) < δ1 (t0 ) ⇒ x(t) → 0 as t → ∞ (4.25)
The set
Bδ1 (t0 ) = x ∈ Rn : x < δ1 (t0 ) (4.26)
is called a region of attraction for the equilibrium 0.

4. Uniformly asymptotically stable over [t0 , ∞) if
1. It is uniformly stable over [t0 , ∞), and
2. There exists a δ1 > 0 such that
x(t0 ) < δ1 ⇒ x(t) → 0 as t → ∞ (4.27)
Moreover, the convergence is uniform with respect to t0 .

5. Globally asymptotically stable if x(t) → 0 as t → ∞ (regardless of what x(t0 )
is, i.e. the region of attraction is the whole Rn ).
6. Unstable if it is not stable at time t0 .
Remark 4.3 Let us try to understand the previous definitions for our advantage in
the control-observer design of robot manipulators. First of all, the very first definition
should be understood in the following way: if it is not wished the norm of the solution
of (4.21), i.e. x(t), to become larger than a predefined value at any time, does
there exist a bound for the initial state x(t0 ) so that if smaller than this value the
norm of the state will never be larger than ? This bound is designated as δ(t0 , )
in Definition 4.8.1, and it is very important to understand that it is not chosen first
and then is computed, but exactly the other way around, if possible, i.e. if the
equilibrium is stable. Therefore, it is possible to propose a priori a region D as
D = x ∈ Rn \ x < , (4.28)
as a region of interest to work in it and then to try to find the initial conditions for
which the equilibrium is stable. Note that it is rather a common practice to infer as
a function of x(t0 ) and that this does not necessarily imply a mistake but usually it
proves to be a more involved procedure. Figure 4.2 illustrates the stability concept.
The bound δ used to define stability in the sense of Lypaunov depends in general
not only on the predefined value but also on the initial time t0 . However, if it is
possible to find a bound for the initial condition which does not depend on t0 for any
t1 ≥ t0 , then Definition 4.8.2 holds, i.e. the equilibrium point is uniformly stable.
This is illustrated in Fig. 4.3.
Till now all what stability of an equilibrium point implies is that the state will
be bounded for all time as long as it is possible to find a bound for the initial
condition small enough according with the desired bound for the state. However,
Definition 4.8.3 makes a stronger implication in case the system is stable (not yet
x2
x(t)
δ(t0 , )
x1
x(t0 )

Fig. 4.2 Stable equilibrium point
uniformly), and that is that the trajectories eventually will go back to the equilibrium
point. Condition 32 of Definition 4.8.3 defines the very important concept of regions
of attraction and it states that if the initial state belongs to the region of attraction,
then it ultimately will return to the origin. Therefore, intuitively Condition 31 appears
to be unnecessary. However, this intuition is wrong. This can easily be understood
by considering a system whose trajectories behave as shown in Fig. 4.4. In fact, just
because x0 ∈ Bδ1 (t0 ) it does not mean that x(t) ∈ Bδ1 (t0 ) for all t ≥ t0 . The trajectory
perfectly well can leave the region, but it will ultimately reenter it and tend to the
origin. If it is chosen = δ1 then the system will not be stable but x(t) will remain
bounded and tend to zero anyway. Of course, for some applications this might just be
good enough, but still technically speaking the equilibrium point will not be stable.
Definition 4.8.4 makes the stability concept independent of t0 as before and it is easy
to understand in view of the previous item.
One of the most beloved concepts in control theory is global asymptotic stability,
given in Definition 4.8.5. It simply states that the trajectory will tend to the origin
regardless the initial value x0 . There are many reasons for which global stability
is desirable. First of all it implies that the system has one single equilibrium point
and second all the system trajectories tend to it. This is in contrast to the previous
definitions, which deal just with the trajectories in a vicinity of the equilibrium
point, however large this vicinity may be. When it comes to design a controller or
an observer for robot manipulators (or any other physical system) it can prove to be
very hard to get global stability. The main premise of this book is that stability, or
local stability as it is also called, can be good enough in practical applications and
thus is a valuable goal on its own that can be easier to achieve.
x2
x(t)
δ()
x1
x(t0 )

Fig. 4.3 Uniformly stable equilibrium point
Finally, Definition 4.8.6 states that if none of the previous definitions hold, then
the equilibrium point is unstable.
4.2.2 Main Stability Theorem
This section presents a very well known result regarding stability in the sense of
Lyapunov. As usual, only the information necessary for the rest of the book is pro-
vided so that many important results are excluded. Consider once again the vector
differential equation (4.21)
ẋ(t) = f (t, x(t)) t ≥ t0 ,
and suppose that for a domain D ⊂ Rn which contains the origin it is possible to find
a continuously differentiable function V (t, x) : [0, ∞) × D → R satisfying
α1 (x) ≤ V (t, x) ≤ α2 (x), (4.29)
∀ t ≥ t0 and ∀ x ∈ D, where α1 and α2 are class K functions. According to Defini-

tion 4.7, V (t, x) is both (locally) positive definite and decrescent. Note that while
V (t, x) may or may not depend explicitly on time t, neither α1 (x) nor α2 (x)
x2
x(t)
x(t0 )
x1
δ=
Fig. 4.4 A bounded state tending to the equilibrium point does not necessarily means asymptotic
stability
V (t, x)
x2
α2 (x)
x
V (t, x)
x1
D α1 (x)
t0 t1 t
Fig. 4.5 V (t, x) bounded by α1 (x) and α2 (x) without considering (4.21) and the equilibrium
x=0
do. This is a key characteristic. Figure 4.5 depicts the concept of V (t, x) bounded by
α1 (x) and α2 (x).
It is important to note that Fig. 4.5 is not completely correct because it shows
bounds without making any association between (4.21) and (4.29) at all. In fact, by
assumption x = 0 is an equilibrium point of (4.21), which means that if at t = t1
x ≡ 0 then x ≡ 0 for all t ≥ t1 .
V (t, x)
x2
α2 (x)
x
V (t, x)
x1
D α1 (x)
t0 t1 t
Fig. 4.6 V (t, x) bounded by α1 (x) and α2 (x) by considering (4.21) and the equilibrium x = 0
Fig. 4.7 The derivative of V (t, x)

V̇ (t, x) indicates whether
V (t, 0) increases, stays or
decreases
V̇ = 0
V̇ > 0
V̇ < 0
t
t0 t1
Figure 4.6 shows the relationship between (4.21) and (4.29) correctly. Note that for
that case α1 (0) = V (t, 0) = α2 (0) = 0 for all t ≥ t1 . The question arises whether one
can conclude any kind of stability from this relationship. In the figures, V (t, 0) going
down to zero apparently indicates asymptotic stability, but how to know whether
V (t, 0) is going down? Recall that V (t, 0) is never negative. Of course, by paying
attention to the derivative V̇ (t, x) as shown in Fig. 4.7.
The derivative of V (t, x) can be computed in general as
∂V ∂V
V̇ (t, x) = + f (t, x), (4.30)
∂t ∂x
where the dependence on (4.21) becomes evident. Prior to drawing out a conclusion
about stability, it should be recalled that just because x → 0 it does not mean that
the equilibrium is stable as defined in Definition 4.8 (see Fig. 4.4). Therefore, the
stability analysis is more complex than just considering the sign of the derivative
V̇ (t, x). The first step is to take into account that the region D does not need to be a
ball as given in (4.28) because in fact it is always possible to take an > 0 to define
a ball contained in D as
x2
B

x1
Fig. 4.8 B ∈ D
B = x ∈ Rn | x ≤ ⊂ D. (4.31)
This is shown in Fig. 4.8.

In practice uniform stability is sought. This means that it must be possible to find
a δ() such that (4.24) holds. Choose
c < α1 (), (4.32)
i.e. choose c strictly smaller than α1 (x) evaluated at the boundary of the ball B .
Since α1 (x) is a class K function, then the region defined as
D1,c = {x ∈ B | α1 (x) ≤ c} (4.33)
must be contained in B as depicted in Fig. 4.9.

In the same fashion a time-dependent region can be defined as
Dt,c = {x ∈ B | V (t, x) ≤ c} . (4.34)
But if V (t, x) ≤ c then α1 (x) ≤ c, so that it can be concluded that Dt,c ⊂ D1,c
because every x ∈ Dt,c is also in D1,c but not necessarily vice versa. This is shown
in Fig. 4.10.
Finally, another region can be defined as
x2
B
x1
D1,c
D
Fig. 4.9 D1,c ⊂ B ∈ D
x2
B
Dt,c
x1
D1,c
D
Fig. 4.10 Dt,c ⊂ D1,c ⊂ B ∈ D

x2
B
Dt,c
D2,c x1
D1,c D
Fig. 4.11 D2,c ⊂ Dt,c ⊂ D1,c ⊂ B ⊂ D
D2,c = {x ∈ B | α2 (x) ≤ c} . (4.35)
For this case, if α2 (x) ≤ c then of course V (t, x) ≤ c, which means that D2,c ⊂
Dt,c as shown in Fig. 4.11. Therefore, it is possible to arrive to the following conclu-
sion
D2,c ⊂ Dt,c ⊂ D1,c ⊂ B ⊂ D, (4.36)
for all t ≥ t0 just based on (4.29). Note that the time-dependent region Dt,c is sur-
rounded by two time-independent surfaces.
Assume now that
V̇ (t, x) ≤ 0 (4.37)
in D. Then, any trajectory beginning in Dt0 ,c must remain in Dt,c for all t ≥ t0 because
for that case
V (t, x) ≤ V (t0 , x0 ) ≤ c ∀t ≥ t0 . (4.38)
Therefore, any solution starting in D2,c remains in Dt,c for all t ≥ t0 as well and, as a
direct consequence, in D1,c ⊂ B for all t ≥ t0 . This necessarily implies that x(t) <
for all t ≥ t0 , which will comply with Definition 4.8.2 as long as a δ() > 0 can
be found to have x(t0 ) < δ() as well. By noting that the condition to define D2,c
in (4.35) can be changed to x ≤ α2−1 (c), choosing
δ() = α2−1 (c) < α2−1 (α1 ()) (4.39)
implies that x(t0 ) ∈ D2,c . Although this choice excludes the boundary of D2,c , this
is of little relevance. Therefore, it has been shown that the equilibrium x = 0 is
uniformly stable. To prove asymptotic stability, the assumption given in (4.37) should
be changed by
V̇ (t, x) ≤ −W3 (x), (4.40)
where W3 (x) is a continuous positive definite function on D. Note that in fact there
exists a class K function α3 (x) such that
V̇ (t, x) ≤ −W3 (x) ≤ −α3 (x), (4.41)
holds, as highlighted in Remark 4.2. From (4.29) one gets
V (t, x) ≤ α2 (x) ⇒ α2−1 (V (t, x)) ≤ x, (4.42)
from where it is possible to get
α3 (α2−1 (V (t, x))) ≤ α3 (x) (4.43)
because α3 (x) is a class K function. Define a new class K function as α4 (·) =

α3 (α2−1 (·)) to obtain from (4.41)
V̇ (t, x) ≤ −W3 (x) ≤ −α4 (V (t, x)). (4.44)
Without any loss of generality it can be assumed that α4 (·) is locally Lipschitz. If it
were not the case, then another class K function α5 (·) satisfying α5 (·) ≤ α4 (·) can
always be found which is locally Lipschitz. Consider now the following differential
equation
ẏ = −α4 (y) y0 = V (t0 , x0 ), (4.45)
whose solution satisfies
y(t) = β1 (V (t0 , x0 ), t − t0 ) ∀ t ≥ t0 , (4.46)
according to Lemma 4.2. β1 (·, ·) is a class KL function defined on [0, c] × [t0 , ∞),
where c has been used to define Dt,c in (4.34). By the Comparison Lemma 4.1 it is
V (t, x) ≤ β1 (V (t0 , x0 ), t − t0 ) ∀ t ≥ t0 , (4.47)

with V (t0 , x0 ) ∈ [0, c]. Therefore, any solution starting in D2,c satisfies
x ≤ α1−1 (V (t, x)) ≤ α1−1 (β1 (V (t0 , x0 ), t − t0 ))

≤ α1−1 (β1 (α2 (x0 ), t − t0 ))
= β(x0 , t − t0 ) (4.48)
for all t ≥ t0 and where β(·, ·) is a class KL function as pointed out in Remark 4.1.
According to Definition 4.8.4 the equilibrium is uniformly asymptotically stable
if it is uniformly stable over [t0 , ∞), which has already been shown, and if there
exists a δ1 > 0 such that x(t0 ) < δ1 ⇒ x(t) → 0 as t → ∞. By choosing also
δ1 = α2−1 (c) as in (4.39) and by noting that β(x0 , t − t0 ) → 0 as t → ∞ because
it is a class KL function, it can be concluded that the equilibrium is uniformly asymp-
totically stable. Finally, if D = Rn and if the functions α1 (·) and α2 (·) belong to the
class K∞ , then the equilibrium is globally uniformly asymptotically stable, because
α1 (·) is radially unbounded and c in (4.32) can be defined arbitrarily large because
can also be considered arbitrarily large, so that δ() = α2−1 (c) becomes arbitrarily
large as well to include any initial condition x(t0 ). Note that α3 (·) becomes a class
K∞ function because W3 (x) is defined in Rn . The following theorem summarizes
the preceding discussion.
Theorem 4.2 Consider the nonautonomous system (4.21), i.e.
ẋ(t) = f(t, x(t)) t ≥ t0 ,
where f : [0, ∞) × D → Rn is piecewise continuous in t and locally Lipschitz in x

on [0, ∞) × D, where D ⊂ Rn is a domain that contains the origin x = 0, which is
assumed to be an equilibrium point of (4.21). Let V (t, x) : [0, ∞) × D → R be a
continuously differentiable function satisfying (4.29), i.e.
α1 (x) ≤ V (t, x) ≤ α2 (x),
where α1 (·), α2 (·) are class K functions. Then, the equilibrium is

1. Uniformly stable if (4.37) holds, i.e. if
V̇ (t, x) ≤ 0.
2. Uniformly asymptotically stable if (4.40) holds, i.e. if
V̇ (t, x) ≤ −W3 (x),
where W3 (x) is a continuous positive definite function on D. Furthermore, if

is chosen such that B ⊂ D and c is chosen such that c < α1 (), then for any
x(t0 ) < α2−1 (c) the solution x(t) satisfies (4.48), i.e.
x(t) ≤ β(x0 , t − t0 )
for all t ≥ t0 , where β(·, ·) is a class KL function.

3. Globally uniformly asymptotically stable if additionally D = Rn and α1 (·), α2 (·)
are class K∞ functions.
Remark 4.4 Defining a positive definite function V (t, x) satisfying (4.29) can prove
to be rather trivial. Therefore, such a function is called Lyapunov candidate function.
If the derivative of the candidate function actually satisfies (4.37) or (4.40) then it
is called a Lyapunov function. Note also that the class K functions in (4.29) can be
replaced by two positive definite functions W1 (x) and W2 (x) leading to the same
conclusions, but making the stability analysis slightly more complex. Since for the
positive definite functions employed is this book it is quite easy to satisfy (4.29), it
has been preferred to use these bounds from the very beginning.
4.2.3 Complementary Results
This section presents also some very well known stability results whose proofs are
omitted for brevity’s sake. First of all, the previous section presented one of the main
results to prove stability in the sense of Lyapunov for nonautonomous systems. It
turns out that establishing stability for autonomous system is even more direct.
Corollary 4.1 Consider the autonomous system
ẋ = f(x), (4.49)
where f : D → Rn is locally Lipschitz in x on D, and D ⊂ Rn is a domain that

contains the origin x = 0, which is assumed to be an equilibrium point of (4.49). Let
V (x) : D → R be a continuously differentiable positive definite function. Then, the
equilibrium is
1. Stable if V̇ (x) is negative semidefinite in D.
2. Asymptotically stable if V̇ (x) is negative definite in D.
3. Globally asymptotically stable if additionally V (x) is radially unbounded and
D = Rn .
Note that the term uniform can be dropped for autonomous systems since they do
not depend explicitly on time t. It can be seen that the condition to achieve asymptotic
stability is still that the derivative of f (x) be negative definite. The following theorem
is based on La Salle’s invariance principle and relaxes this condition.
Theorem 4.3 Consider the autonomous system (4.49), where f : D → Rn is locally

Lipschitz in x on D, and D ⊂ Rn is a domain that contains the origin x = 0, which
is assumed to be an equilibrium point of (4.49). Let V (x) : D → R be a continu-
ously differentiable positive definite function. Let M = {x ∈ D | V̇ (x) = 0}. If the
only solution x that can stay identically in M is the equilibrium x ≡ 0, then the
equilibrium is
1. Asymptotically stable if V̇ (x) is negative semidefinite in D.
2. Globally asymptotically stable if additionally V (x) is radially unbounded and
D = Rn .
One of the most important particular cases of autonomous systems are time invari-
ant linear systems. The following theorem provides a very well known result.
Theorem 4.4 Consider the linear time invariant autonomous system
ẋ = Ax, (4.50)
where A ∈ Rn×n is a constant matrix. The equilibrium point x = 0 is globally asymp-

totically stable if and only if all the eigenvalues of A have negative real part, i.e.
Re(λi (A)) < 0 for i = 1, . . . , n.
Finally, since the previous results applies only for autonomous systems, a very
important and well known result is given below.
Lemma 4.5 (Barbalat’s) If a differentiable function f (t) has a finite limit as t →
∞, and if f¨(t) is bounded, then f˙(t) → 0 as t → ∞.
The usefulness of Barbalat’s Lemma may not be evident at first sight. However,
on the contrary to what intuition might just say, the fact that lim f (t) exists does
t→∞
not necessarily imply that lim f˙(t) = 0. A common example is f (t) = sin(tt ) . The
2
t→∞
contrary is also true, i.e. if lim f˙(t) = 0 then lim f (t) does not necessarily exists,
t→∞ t→∞
e.g. f (t) = sin(ln(t)).
4.3 Ultimate Boundedness
4.3.1 Definition
Lyapunov stability intrinsically implies boundedness of the state. In the previous

section it was mentioned that choosing x0 = 0 as equilibrium point did not only
not imply any loss of generality, but it is even convenient because one of the most
important goals in control design is to make the error state to tend to zero. However,
what if this is not possible or if there is no equilibrium point? Is x0 ≈ 0 acceptable
instead? In practice, due to a wide variety of reasons, one may actually get x0 ≈ 0
even if the equilibrium is (theoretically) asymptotically stable. On the other hand, if
achieving x0 → 0 is not possible, then obtaining on purpose x0 ≈ 0 in a finite time
can just be acceptable. But x0 ≈ 0 is by no means an accurate concept, so that it is
better to use the expression x0 ≤ bf for some positive scalar bf which does not
need to satisfy bf ≈ 0. With this is mind, consider the following definition.
4.3 Ultimate Boundedness 121
Fig. 4.12 A uniform x2

ultimate bounded trajectory
with ultimate bound bf
x(t)
x(t0 )
bf
a x1
Definition 4.9 The solutions of (4.21), i.e. ẋ(t) = f (t, x(t)), are said to be
1. Uniformly bounded if there exist a constant c > 0 such that for every a ∈ (0, c)
exists a positive constant b depending only on a, i.e. b = b(a) > 0, such that
x(t0 ) ≤ a ⇒ x(t) ≤ b ∀ t ≥ t0 (4.51)
2. Globally uniformly bounded if (4.51) holds for every a ∈ (0, ∞).

3. Uniformly ultimately bounded with ultimate bound bf if besides c > 0 exists a
positive constant bf > 0, such that for every a ∈ (0, c) exists a T = T (a, bf ) ≥ 0,
independent of t0 , such that
x(t0 ) ≤ a ⇒ x(t) ≤ bf ∀ t ≥ t0 + T (4.52)
4. Globally uniformly ultimately bounded if (4.52) holds for every a ∈ (0, ∞).
4.3.2 An Ultimate Boundedness Theorem
For the purpose of the present book, Definition 4.9.3 is of particular interest.
Figure 4.12 depicts the concept, where it is desirable to make b arbitrarily large
and bf arbitrarily small by design. Although at first sight these two goals may appear
to be easier to achieve than asymptotic stability, they represent an important chal-
lenge on its own. Note that in fact the region D in (4.28) could be also used here but
with the bound b instead of . Since there is also a bound for the initial condition
involved in (4.51), this looks very similar to the stability definition. However, it is
conceptually different since no equilibrium point is involved.
V (t, x)
x2
α2 (x)
x
V (t, x)
x1
D Vb
α1 (x)
t0 t
Fig. 4.13 V (t, x) bounded by α1 (x) and α2 (x) under condition (4.53)
In order to get some insight about the conditions for a trajectory to be uniformly
ultimately bounded, the concept of positive definite functions used to prove stability
can further be exploited. Indeed, consider once again a continuously differentiable
function V (t, x) : [0, ∞) × D → R satisfying (4.29), i.e.
α1 (x) ≤ V (t, x) ≤ α2 (x),
∀ t ≥ t0 and ∀ x ∈ D, where as before D ⊂ Rn is a domain that contains the origin.

Consider also the vector differential equation (4.21), i.e.
ẋ(t) = f (t, x(t)) t ≥ t0 ,
whose behavior should be related to V (t, x). Note that this time it is not assumed that
the origin is an equilibrium of (4.21), although this could well be the case. Consider
a similar condition to that given in (4.40), namely
V̇ (t, x) ≤ −W3 (x), ∀ x ≥ μ > 0, (4.53)
where W3 (x) is a continuous positive definite function on D. As can be appreciated,

the one single difference between (4.40) and (4.53) is that the derivative of V (t, x) is
not negative for every x ∈ D, but only for those solutions satisfying x ≥ μ > 0.
By taking into consideration (4.29), the behavior of V (t, x) should be similar to that
shown in Fig. 4.13.
Whenever V̇ (t, x) is negative the magnitude of V (t, x) decreases. On the contrary,
in general the magnitude of x may not decrease immediately just because V̇ (t, x) <
0, but eventually it will if V̇ (t, x) is negative for enough time. Therefore, eventually
x < μ and for that case V̇ (t, x) becomes indefinite. However, there must exist a
value Vb which once reached can never be exceeded, as depicted in Fig. 4.13. Since
from (4.29) it is known that α1 (x) ≤ V (t, x) then necessarily x becomes ultimately
bounded. Note that a similar reasoning led to Theorem 4.2 (just by setting μ ≡ 0
would be the very same), so that some steps of the proof can be employed to get a
result for uniform ultimate boundedness. First of all, let’s make a small change in
the definition of the constant c given in (4.32)
c = α1 (), (4.54)
where as before the constant is used to define the region B in (4.31) so that B ⊂ D.
As mentioned before, just because V̇ (t, x) is negative it does not mean that x(t)
decreases immediately and in fact x could leave D if x0 lies too close to the boundary.
Therefore, assume that
α2 (μ) < c ⇒ μ < α2−1 (c) ⇒ μ < α2−1 (α1 ()). (4.55)
In the very same fashion, assume that
α2 (x0 ) ≤ c ⇒ x0 ≤ α2−1 (α1 ()). (4.56)
It is not difficult to show that if (4.29) and (4.53)–(4.56) hold, then any solution
of (4.21) remains in D for all t ≥ t0 . To show this, note that if x0 < μ at t = t0
then (4.53) does not hold and V̇ (t, x) is indefinite. However, it would be of advantage
if it were positive or even zero, so that the worst possible situation is that V̇ (t, x) > 0
and V (t, x) grows until x(t1 ) ≡ μ at some finite time t1 > t0 . At that moment (4.53)
holds and one has V̇ (t, x) ≤ −W3 (x) so that V (t, x) decreases until once again
x(t) < μ and V̇ (t, x) becomes indefinite, and the sequence starts over. On the
other hand, if x0 ≥ μ then V (t, x) decreases until x(t) < μ and the sequence
starts over as well. Therefore, one can fairly assume that x0 = α2−1 (α1 ()) so
that (4.53) holds at t = t0 and that would be the worst possible case because V (t, x)
begins decreasing its value. In view of the previous discussion, a region can be defined
as done in (4.34)
Dt,c = {x ∈ B | V (t, x) ≤ c = α1 ()} ,
where it is guaranteed that any trajectory starting in Dt,c remains inside because
V̇ (t, x) is negative at the boundary. Note that in order for x to be in Dt,c then x ≤
as can be concluded when V (t, x) = c = α1 () holds. Indeed, by considering (4.56)
α2 (x0 ) ≤ c = α1 () ⇒ x0 ∈ Dt0 ,c ⊂ Dt,c ∀t ≥ t0 . (4.57)
It is also important that if x0 = α2−1 (α1 ()) then from (4.29) it is
α1 (x(t)) ≤ V (t, x) ≤ V (t, x0 ) ≤ α2 (x 0 ) = α1 (), (4.58)
which implies that
x(t) ≤ . (4.59)
As explained before V (t, x) decreases until x(t) < μ, but if (4.53) holds again
because x(t) ≥ μ, then for that case μ can be considered as a new initial condition
norm value and one would have
α1 (x(t)) ≤ V (t, x) ≤ α2 (μ), (4.60)
and consequently
x(t) ≤ α1−1 (α2 (μ)) < . (4.61)
The last inequality follows after (4.55). In the end this implies that x(t) ≤ for all
t ≥ t0 . This comply with Definition 4.9.1 (uniform boundedness) by choosing b =
and a = α2−1 (α1 ()). Note that if D = Rn and α1 (·) and α2 (·) are class K∞ functions,
then Definition 4.9.2 (global uniform boundedness) holds as well. It remains to show
ultimate boundedness. Consider
η = α2 (μ) < c, (4.62)
and define
Bμ = {x ∈ B | α2 (x) ≤ α2 (μ)} (4.63)

Dt,η = {x ∈ B | V (t, x) ≤ η = α2 (μ)} (4.64)
D1,η = {x ∈ B | α1 (x) ≤ η < α1 ()} . (4.65)
By noting that D1,c = B since c = α1 (), then one can conclude that
Bμ ⊂ Dt,η ⊂ D1,η ⊂ Dt,c ⊂ D1,c = B ⊂ D, (4.66)
just by following the same arguments to get (4.36), where Bμ takes the place of D2,c
and implies x ≤ μ and therefore at the boundary and outside this region (4.53)
holds. This means that every trajectory starting in Dt,η remains inside, but since Dt,η
is a subset of Dt,c , then any trajectory starting inside Dt,c but outside Dt,η must enter
in this last region in a finite time because for that case
V̇ (t, x) ≤ −W3 (x). (4.67)
Otherwise said, (4.67) holds in the region Dt,c − Dt,η . But in view of the definition
of Dt,η , any x in this region must comply with x ≤ μ. Therefore, by recalling that
any x in Dt,c satisfies x ≤ , (4.67) holds for μ ≤ x ≤ , and over this set it is
possible to define k = min(W3 (x)), such that
V̇ (t, x) ≤ −k < 0 (4.68)
holds. By integrating one gets

V (t, x) ≤ V (t, x0 ) − k(t − t0 ) ≤ c − k(t − t0 ). (4.69)
This last relationship will be valid until V (t, x) = η, which must be reached in a
finite time tη obtained below
c−η
η ≤ c − k(tη − t0 ) ⇒ tη ≤ t0 + . (4.70)
k
Assume that T is the first time that x enters in Dt,η , so that in the time interval [t0 , T ]
must hold
V̇ (t, x) ≤ −W3 (x) ≤ −α3 (x) ≤ −α3 (α2−1 (V (t, x))) (4.71)
for a class K function α3 (·) and where V (t, x) ≤ α2 (x) has been taken into account.
Therefore, it is possible to recover (4.44), i.e.
V̇ (t, x) ≤ −α4 (V (t, x)),
with α4 (·) a class K function. Also as before one recovers (4.47), i.e.
V (t, x) ≤ β1 (V (t0 , x0 ), t − t0 )
but for t ∈ [t0 , t0 + T ] and (4.48) as direct consequence, i.e.
x ≤ β(x0 , t − t0 ) ∀ t ∈ [t0 , t0 + T ], (4.72)
where β1 (·, ·) and β(·, ·) are class KL functions. Since once in Dt,η the solution
remains inside, then for t ≥ t0 + T , relation (4.61) holds, i.e.
x(t) ≤ α1−1 (α2 (μ)) = α1−1 (η). (4.73)
Note that if x0 ∈ Dt,η then (4.61) holds for all t ≥ t0 . Therefore, this complies
with Definition 4.9.3 (uniform ultimate boundedness) by choosing bf = α1−1 (α2 (μ))
while (4.70) shows that T must exist. Finally, if D = Rn and α1 (·) and α2 (·) are class
K∞ functions, then Definition 4.9.4 (global uniform ultimate boundedness) holds as
well. The previous discussion is summarized in the following theorem.
Theorem 4.5 Consider the nonautonomous system (4.21), i.e.
ẋ(t) = f(t, x(t)) t ≥ t0 ,
where f : [0, ∞) × D → Rn is piecewise continuous in t and locally Lipschitz in x on

[0, ∞) × D, where D ⊂ Rn is a domain that contains the origin x = 0. Let D ⊂ Rn
be a domain that contains the origin and V : [0, ∞) × D → R be a continuously
differentiable function such that (4.29) and (4.53) hold, i.e.
α1 (x) ≤ V (t, x) ≤ α2 (x)

V̇ (t, x) ≤ −W3 (x), ∀ x ≥ μ > 0,
∀ t ≥ 0 and ∀ x ∈ D, where α1 and α2 are class K functions, W3 (x) is a continuous

positive definite function. Take > 0 such that B = {x ∈ Rn | x ≤ } ⊂ D and
suppose that (4.55) holds, i.e.
μ < α2−1 (α1 ()).
Assume that (4.56) holds, i.e.
x(t0 ) = x0 ≤ α2−1 (α1 ()).
Then, there exits a class KL function β(·, ·) and a T ≥ 0 such that the solution
of (4.21) satisfies (4.72) and (4.73), i.e.
x ≤ β(x(t0 ), t − t0 ), ∀ t0 ≤ t ≤ t0 + T

x ≤ α1−1 (α2 (μ)), ∀ t ≥ t0 + T.
Moreover, if D = Rn and α1 belongs to class K∞ , then (4.72)–(4.73) hold for any

initial state x(t0 ), with no restriction on how large μ is.
4.4 Sliding Surfaces
One of the main goals of robot control theory is trajectories tracking. The idea is to
provide a path and orientation for the robot end-effector which has to be followed as
close as possible. The desired values can be denoted as qd and q̇d , so that the tracking
error can be defined as
e = q − qd . (4.74)
Therefore, to apply the theory of the previous section the state of the error dynamics
can be defined as
e
x= . (4.75)
ė
Alternatively, a sliding surface s(t) = 0 in Rn can be defined. For simplicity’s sake

assume first that n = 1, so that the sliding surface is given by
m−1
d
s(x, t) = +λ e, (4.76)
dt
4.4 Sliding Surfaces 127
where λ is a positive constant and m ≥ 2 is a positive integer. For this book, the case
of special interest is m = 2 to get
s = ė + λe. (4.77)
Clearly, this represents a stable linear system in the form of (4.50) but with an external
input, i.e.
ė = −λe + s, (4.78)
where A = −λ and therefore it is stable. By properties of stable linear systems, if s

is bounded, then so are e and ė. Furthermore, if s → 0 then e, ė → 0 as well. In fact,
if s ≡ 0, then system (4.78) is autonomous and evidently e, ė → 0. Thus, one can
replace the problem of achieving e, ė → 0 by getting s → 0 as t → ∞ or if possible
s ≡ 0 from a finite time tr . It can be shown that the latter is possible if the so called
sliding condition is matched, i.e.
1 d 2
s ≤ −η|s|, (4.79)
2 dt
where η > 0. Satisfying this condition makes the surface an invariant set and the
behavior of the system once the sliding surface is reached is known as sliding mode.
Condition (4.79) guarantees that s ≡ 0 in a finite time tr in case s(t0 ) = 0. To show
this, assume without lost of generality that t0 = 0 and that s(t = 0) > 0, so that
from (4.79) one gets between t = t0 and s(t = tr ) = 0
s ṡ ≤ −ηs ⇒ ṡ ≤ −η. (4.80)
By integrating it is
t t
s 0r ≤ −ηt 0r ⇒ s(t = tr ) − s(t = 0) = 0 − s(t = 0) ≤ −ηtr , (4.81)
which in turn implies that
ηtr ≤ s(t = 0), (4.82)
and therefore
s(t = 0)
tr ≤ . (4.83)
η
Note that the result would be completely similar if s(t = 0) < 0, from which it is
concluded
|s(t = 0)|
tr ≤ . (4.84)
η
Just as explained before, s ≡ 0 implies that e, ė → 0, but satisfying the sliding con-
dition may require the use of discontinuities in the control law, which is prone to
cause chattering. Finally, the result can easily be extended to the vector case
s = ė + e, (4.85)
with ∈ Rn×n a diagonal positive definite matrix.
References
Desoer CA, Vidyasagar M (1975) Feedback systems: input-output properties. Academic, New York
Hahn W (1963) Theory and applications of Liapunov’s direct method. Prentice-Hall, Englewood
Cliffs
Hahn W (1967) Stability of motion. Springer, New York
Khalil HK (1996) Nonlinear sstems, 2nd edn. Prentice-Hall, Englewood Cliffs
Khalil HK (2002) Nonlinear systems, 3rd edn. Prentice-HallPrentice-Hall, Upper Saddle River
Nuño E, Basañez L, Ortega R, Spong MW (2009) Position tracking for non-linear teleoperators
with variable time delay. Int J Robot Res 28(7):895–910
Nuño E, Basañez L, López-Franco C, Arana-Daniel N (2014) Stability of nonlinear teleoperators
using PD controllers without velocity measurements. J Franklin Inst 351:241–258
Skelton RE (1988) Dynamic systems control: linear systems analysis and synthesis. Wiley, New
York
Slotine JJE, Li W (1991) Applied nonlinear control. Prentice-Hall, Englewood Cliffs
Vidyasagar M (1978) Nonlinear systems analysis. Prentice-Hall, Englewood Cliffs
Chapter 5
Common Control Approaches for Robot
Manipulators
Abstract Robot manipulators are designed to carry out specific tasks but at the same
time to be flexible enough to allow reprogramming if necessary. Since the variety of
possible works is too wide, the most direct way to obtain a general solution to carry
them out is to propose a time varying desired trajectory in terms of joint or work space
coordinates and then design a control law able to follow it. From a pure theoretical
point of view achieving this goal can even be trivial despite the high nonlinear
dynamics of robot manipulators. However, when it comes to implementation many
practical issues arise that make the problem much more involved than it appears to
be at the beginning. This chapter recapitulates the most basic control techniques for
robot manipulators.
5.1 PD and PD+ Control
Despite the high nonlinear dynamic nature of robot manipulators, it turns out that
some tasks can be carried out rather straightforwardly and let’s say easily. Assume
that it is desired to take the robot manipulator to its initial position q(0) = q 0 to a
final constant position q d ∈ Rn . The position error is therefore here and for the rest
of the book given by
e = q − qd. (5.1)
If velocity measurements are available, then a PD control law can be implemented

as
τ = −K d ė − K p e, (5.2)
where K d , K p ∈ Rn×n are diagonal positive definite matrices. Now, for simplicity’s
sake assume that g(q) = 0 and D = O in model (3.44). For that case one has the
closed loop dynamics given by
H(q)ë + C(q, q̇)ė + K d ė + K p e = 0, (5.3)

https://doi.org/10.1007/978-3-030-85980-0_5
130 5 Common Control Approaches for Robot Manipulators
where it has been taken advantage of the fact that q̇ d ≡ 0 ⇒ ė = q̇. Consider the
following Lyapunov candidate function
1 T 1
V (e, ė) = ė H(q)ė + eT K p e. (5.4)
2 2
The corresponding derivative along (5.3) is then given by
1
V̇ (e, ė) = ėT H(q)ë + ėT Ḣ(q)ė + ėT K p e (5.5)
2
1
= −ėT K d ė + ėT Ḣ(q) − 2C(q, q̇) ė + ėT K p e − ėT K p e
2
1
= −ėT K d ė + ėT Ḣ(q) − 2C(q, q̇) ė.
2
According to Property 3.2 one has
V̇ (e, ė) = −ėT K d ė. (5.6)
Since V̇ (e, ė) is only negative semidefinite, Theorem 4.3 can be used. In fact,
V̇ (e, ė) ≡ 0 in the set
M = {(e, ė) : ė ≡ 0}. (5.7)
Therefore, one only needs to show that the only solution (e, ė) that can stay identically
in M is the equilibrium (e, ė) ≡ (0, 0). This is carried out by substituting M, i.e.
ė ≡ 0, in (5.3) to get
K p e = 0.
Since K p is positive definite its inverse exists and therefore the only solution for the
previous relationship is precisely e = 0, which shows that the origin is asymptotically
stable. But since furthermore V (e, ė) is radially unbounded in view of Property 3.7,
then one can conclude from Theorem 4.3 that it is globally asymptotically stable.
Note that neither K d nor K p need to be diagonal, but this is a common practice and
also simplifies gain tuning.
Although regulation on its own may be a very important goal, trajectory tracking is
also essential. For that case q̇ d = 0. Also, in general viscous friction can be neglected,
but gravity effects cannot if they exist. Assuming that the whole robot model is known,
then a control law that can achieve exact tracking is given by
τ = H(q)q̈ d + C(q, q̇)q̇ d + D q̇ d + g(q) − K d ė − K p e, (5.8)

5.1 PD and PD+ Control 131
which is referred to as augmented PD or PD+. Substituting (5.8) in (3.44) leads

practically to the same closed loop equation (5.3), i.e.
H(q)ë + C(q, q̇)ė + K D ė + K p e = 0, (5.9)
where K D = D + K d > O. Therefore, the proof to show that e → 0 and ė → 0 is

the same as before.
5.2 PID Control of Robot Manipulators
Suppose that the gravity vector g(q) is not well known and that it is desired to achieve
position regulation. Consider once again the error e given in (5.1) with q d constant.
Since q̇ d = q̈ d = 0, one also has ė = q̇ and ë = q̈. For that case a PD controller is
not good enough, but it can be shown that a PID of the form
τ = −K p e − K d ė − K i ξ (5.10)
ξ̇ = e, (5.11)
can fulfill this goal, as long as the (diagonal) positive definite matrices K p , K d , K i ∈
Rn×n are properly chosen. The result turns out to be only locally uniformly asymp-
totically stable although the region of attraction can be arbitrarily enlarged. The usual
approach to prove stability is rather complex in the sense that the Lyapunov candidate
function contains both cross terms and the robot’s potential energy; in fact, it is even
involved to find conditions to guarantee that it is positive definite in the first place.
Roughly speaking, the proportional and derivative gains need to be large enough,
and the integral gain needs to be small enough. In this book, an alternative approach
is provided to arrive basically to the same result. Define a vector version of s given
in (4.77) as
s = ė + e ⇒ ė = −e + s, (5.12)
where ∈ Rn×n is a positive definite diagonal matrix. The elements of s can be

considered as the inputs of n stable independent linear filters of the form (4.77)
whose outputs are the corresponding elements of e, so that if it is bounded and tends
to zero, so will do e and ė, i.e.
s→0 ⇒ e, ė → 0. (5.13)
In terms of s, the regular PID control law (5.10)–(5.11) can be rewritten as
τ = −K d s − K ω ω − K ps e (5.14)
ω̇ = s, (5.15)
as long as the following gains equivalence is used
K p = K d + K ω + K ps (5.16)
Kd = Kd (5.17)
K i = K ω . (5.18)
Note that given any arbitrary set K p , K d and K i it is not always possible to get an
equivalent set of gains K d , and K ω . For that reason, the latter have to be chosen
first in order to get the former for implementation.
Substituting (5.14) in (3.44) yields
H(q)ṡ + C(q, q̇)s + K d s + K ω ω + K ps e + g(q) = δ a , (5.19)
with
δ a = H(q)ė + C(q, ė)e, (5.20)
and where D = O has been taken for simplicity. By choosing

⎡ ⎤
ω
y = ⎣ e ⎦, (5.21)
s
where ω is defined in (5.15), it is possible to rewrite (5.19) as

⎡ ⎤
s
ẏ = ⎣ −e + s ⎦. (5.22)
−H −1 (q) C(q, q̇)s + K d s + K ω ω + K ps e + g(q) − δ a
It can be seen that the equilibrium point of (5.22) is e = s = 0 and ω = −K −1

ω g(q d ).
A change of coordinates can be done in the form
x 1 = ω + K −1
ω g(q d ), (5.23)
to have an equivalent system for (5.22) as

⎡ ⎤
s
ẋ = ⎣ −e + s ⎦,
−H −1 (q) C(q, q̇)s + K d s + K ω x 1 + K ps e + g(q) − g(q d ) − δ a
(5.24)
5.2 PID Control of Robot Manipulators 133
with
⎡⎤ ⎡ ⎤
x1 x1
x = ⎣ x2 ⎦ ≡ ⎣ e ⎦ . (5.25)
x3 s
Clearly, the equilibrium point of (5.24) is x = 0 and it is unique. Also note that δ a
in (5.20) can be considered as a vanishing perturbation at the origin.
To prove that x → 0 it is taken advantage of the fact that it is already known that
the origin is uniformly asymptotically stable but not globally. Therefore, the region of
interest (4.28) mentioned in Remark 4.3 can be used to simplify the stability analysis,
i.e.

D = x ∈ Rn \ x < ,
for an arbitrarily large > 0. Consider the following positive definite function V (x)
with respect to the state variables of system (5.24)
1 T 1 1
V (x) = x K ω x 1 + eT K ps e + sT H(q)s. (5.26)
2 1 2 2
V (x) satisfies (4.29) with
α1 (x) = λ1 x2 and α2 (x) = λ2 x2 , (5.27)

λ1 = 21 min λmin (K ω ), λmin (K ps ), λh , λ2 = 21 max λmax (K ω ), λmax (K ps ), λH , and
λh , λH given in Property 3.7. The derivative of (5.26) along (5.24) can easily be com-
puted as
V̇ (x) = −eT K ps e − sT K d s − sT (g(q) − g(q d )) + sT δ a , (5.28)
just by using Property 3.2. To further manipulate (5.28), take Properties 3.9 and 3.11
into account to get by using (5.20)
V̇ (x) ≤ − λmin (K ps )e2 − λmin (K d )s2 + kg es (5.29)

+ λH λmax ()ės + kc λmax ()ėes.
Define the auxiliary vector
e x2
xa = = . (5.30)
s x3
Then, directly from (5.12) one has ė ≤ λmax ()e + s, so that (5.29) becomes
V̇ (x) ≤ − λmin (K ps )e2 − λmin (K d )s2 + kg es (5.31)

+ λH λmax () es + λH λmax ()s
2 2
+ kc λmax ()2 ees + kc λmax ()ses

≤ − λmin (K ps )e2 + γ2 es − λmin (K d )s2 + γ1 s2 + γ3 x a 3 ,
where
γ1 = λH λmax () (5.32)

γ2 = kg + λH λmax () 2
(5.33)
γ3 = kc λmax () + kc λmax ().
2
(5.34)
Assume now that
λmin (K d ) ≥ γ1 + γ + 1, (5.35)
1
λmin (K ps ) ≥ γ + γ22 , (5.36)
4
for some γ > 0, so that V̇ (x) satisfies
V̇ (x) ≤ −γ e2 − γ s2 + γ3 x a 3 . (5.37)
Therefore it is
V̇ (x) ≤ −γ x a 2 + γ3 x a 3 = −W3 (x). (5.38)
To show that W3 (x) is positive semidefinite in D, rewrite (5.38) as
γ3
V̇ (x) ≤ −γ x a 2 1 − x a = −W3 (x). (5.39)
γ
Then, W3 (x) ≥ 0 and V̇ (x) ≤ 0 as long as

γ
x a ≤ . (5.40)
γ3
Finally assume that

γ
< , (5.41)
γ3
5.2 PID Control of Robot Manipulators 135
so that V̇ (x) ≡ 0 only in the set
M = {(x 1 , x a ) : x a ≡ 0 ⇒ (e, s) ≡ (0, 0)}. (5.42)
Then, Theorem 4.3 can be used to show that the origin is (locally uniformly) asymp-
totically stable by noting that in M the only solution x of (5.24) that can stay
identically is precisely the equilibrium x ≡ 0. Note that despite V (x) is radially
unbounded the result cannot be global because D cannot be equal to Rn . By using
the proposed approach it can be concluded, roughly speaking, that stability will be
guaranteed if the derivative and the proportional gains are big enough and gains are
chosen according to (5.16)–(5.18). Note that technically this does not necessarily
mean that the equilibrium of the original error state (ξ , e, ė) is asymptotically stable.
However, it can be shown that indeed this is the case by using directly Definition 4.8
and some equivalences. This proof is omitted because in the end the main goal has
already been shown, i.e. (e, ė) → (0, 0).
5.3 Computed Torque Control
While PID control can compensate the gravity term for position regulation, there
will always be a residual error for trajectory tracking. It has been already shown that
a PD+ control law can achieve this goal, but it is not the only model based technique
which can be employed. One of the earliest and better known results is the computed
torque control law given by

τ = H(q) q̈ d − K d ė − K p e + C(q, q̇)q̇ + D q̇ + g(q), (5.43)
with e given in (5.1) and K d , K p ∈ Rn×n are as before diagonal positive definite gain
matrices. Substituting (5.43) into (3.44) yields

H(q) ë + K d ė + K p e = 0. (5.44)
But since from Property 3.1 the inverse of H(q) always exists and hence
ë + K d ė + K p e = 0. (5.45)
To analyze the closed loop stability properties of control law (5.43), one just needs
to take advantage of the fact that K d , K p > O and diagonal, so that there are n
independent second order linear differential equations of the form
ëi + kdi ėi + kpi ei = 0, (5.46)
where ei is the ith element of e and kdi , kpi are the ith elements of the diagonals of
K d and K p , respectively, for i = 1, . . . , n. The associated characteristic equations
of (5.46) are of the form
λ2 + kdi λ + kpi = 0. (5.47)
Since the eigenvalues of (5.47) do have negative real part because kdi , kpi > 0, then
it is clear that ėi , ei → 0 or ė, e → 0 for any initial condition. It is worthy pointing
out that K d and K p do not need to be diagonal to obtain the same result. Finally, it
is possible to write (5.43) as

τ = H(q)q̈ d + C(q, q̇)q̇ + D q̇ + g(q) + H(q) −K d ė − K p e . (5.48)

τ ff τ fb
The term τ ff is known as the predictive part and it is aimed at providing the necessary
input torque to keep the system along the nominal trajectory. τ fb is the feedback part,
which provides the necessary input torque to reduce tracking errors.
5.4 Exploiting the Passive Structure of Robot Manipulators
The main disadvantage of the computed torque control law is that it relies on an
exact knowledge of the robot model, which makes it little robust against model
uncertainties. This effect is accentuated because the predictive term ideally cancels
out the robot nonlinear dynamics. Instead, the passive structure of robot manipulators
can be exploited to achieve trajectory tracking in a more robust way. Consider the
definition of the reference velocity as
q̇ r = q̇ d − e, (5.49)
so that the vector s in (5.12) can be obtained as
s = q̇ − q̇ r = ė + e. (5.50)
Then, define the control law
τ = H(q)q̈ r + C(q, q̇)q̇ r + D q̇ r + g(q) − K d s, (5.51)
with K d , ∈ Rn×n diagonal and positive definite as usual. Substituting in (3.44)

leads to
H(q)ṡ + C(q, q̇)s + K D s = 0, (5.52)

5.4 Exploiting the Passive Structure of Robot Manipulators 137
where K D = D + K d . Consider the following candidate Lyapunov function1
1 T
V (t) = s H(q)s, (5.53)
2
whose derivative along (5.52) is trivially given by
1 T
V̇ (t) = sT H(q)ṡ + s Ḣ(q)s (5.54)
2
1 T
= −sT K D s + s Ḣ(q) − 2C(q, q̇) s
2
= −sT K D s ≤ 0.
Since K D is positive definite, this means that V̇ (t) is negative definite. Therefore,
by applying Corollary 4.1 it can be concluded that the equilibrium s = 0 of (5.52)
is globally asymptotically stable because additionally V (t) is radially unbounded.
This in turn means that e, ė → 0.
5.5 Design in Work Space Coordinates
In the previous sections, the desired trajectories for the robot manipulators are des-
ignated as q d . However, when it comes to choose these desired values, in practice
they are given in work space coordinates and therefore inverse kinematic online has
to be implemented together with the control law as shown in Fig. 5.1.
The computation of inverse kinematics is explained in Section 2.3. An alternative
to computing the inverse kinematic consists in rewriting model (3.44) directly in work
space coordinates. To do that, assume that f : Q → R p is a smooth and invertible
map from joint variables q ∈ Q to work space variables x ∈ R p . To simplify the
discussion, assume that n = p so that the number of degrees of freedom of the robot
matches the dimension of the work space. If n < 6, then the work space variables
provide only a partial parametrization of S E(3). The transformation is carried out by
using the Jacobian of the manipulator, either the analytic or the geometric. Consider
for example the relationship (2.183)
∂f
ẋ = J(q)q̇ J(q) = ,
∂q
1 Care should be taken! V (t) in (5.53) is a candidate Lyapunov function for s, but not for (e, ė).
from which it can be inferred that

d −1
q̇ = J −1 ẋ ⇒ q̈ = J −1 ẍ + J ẋ. (5.55)
dt
The previous relationship is valid if and only if J −1 (q) exits, which implies that the
robot does not reach any singularity. This is hardly a disadvantage because singu-
larities should also be avoided to compute the inverse kinematics and therefore this
T
assumption is always made. By defining J −T = J −1 , one can get from (3.44):
d −1
J −T H J −1 ẍ + J −T C J −1 + H J ẋ + J −T D J −1 ẋ + g = J −T τ ,
dt
(5.56)
where the dependence on (q, q̇) has been omitted for simplicity. Define
H̄(q) = J −T (q)H(q) J −1 (q) (5.57)

d −1
C̄(q, q̇) = J −T (q) C(q, q̇) J −1 (q) + H(q) J (q) (5.58)
dt
D̄(q) = J −T (q) D J −1 (q) (5.59)
−T
ḡ(q) = J (q)g(q) (5.60)
−T
F= J (q)τ , (5.61)
so that the robot dynamics expressed in work space coordinates is given by
H̄(q) ẍ + C̄(q, q̇) ẋ + D̄(q) ẋ + ḡ(q) = F. (5.62)
This model is mainly used for control design and therefore it is unnecessary to make
the substitution of q and q̇ in terms of x and ẋ. On the other hand, it is not difficult
to show by direct computation that H̄(q) is symmetric positive definite, while as
˙
before H̄(q) − 2 C̄(q, q̇) ∈ Rn×n is skew symmetric. This is very important because
these were two of the key properties used to prove stability in the proposed control
schemes of this section and they could be implemented just by rewriting them in terms
of model (5.62). For example, the computed torque control law can be rewritten as

F = H̄(q) ẍ d − K d ẋ − K p x + C̄(q, q̇) ẋ + D̄ ẋ + ḡ(q), (5.63)
where K d , K p ∈ Rn×n are diagonal positive definite gain matrices and x d ∈ Rn is

the desired trajectory in work space coordinates. Recall that x d has to be provided
anyway as shown in Fig. 5.1. The corresponding tracking error is defined as
x = x − x d . (5.64)
5.5 Design in Work Space Coordinates 139
Fig. 5.1 Implementation of

a control law together with
inverse kinematics online
computation τ
Controller Robot q, q̇
q d , q̇ d , q̈ d xd , ẋd , ẍd
Inverse
kinematics
Desired workspace
coordinates
It is important to note that for implementation it is the input torque which has to be
programmed as
τ = J T (q)F. (5.65)
The stability proof is omitted in view of its obvious similarity with the regular com-
puted torque law.
References
1. Arteaga-Pérez MA (2019) An alternative proof to the asymptotic stability of PID controllers for
regulation of robot manipulators. IFAC J. Syst. Control 9, Article 100066
2. Canudas de Wit C, Siciliano B, Bastin G (eds) (1996) Theory of robot control. Springer, London
3. Kelly R, Santibáñez V, Loria A (2005) Control of robot manipulators in joint space. Springer,
London, Great Britain
4. Murray RM, Li Z, Sastry SS (1994) A mathematical introduction to robotic manipulation. CRC
Press, Boca Raton
5. Siciliano B, Khatib O (eds) (2008) Springer handbook of robotics. Springer, Berlin
6. Slotine JJE, Li W (1987) On the adaptive control of robot manipulators. Int J Robot Res 6(3):49–
59
Part II
Looking for Semiglobal Stability
or Ultimate Boundedness
Chapter 6
Velocity Observer Design
Abstract As seen in Chap. 5, robot control design may be quite direct under ideal
circumstances. Particularly, an exact robot model and joint velocities measurements
should be available in order to implement different effective control schemes. How-
ever, in practice neither of them are available. This chapter deals with velocity esti-
mation. It is worthy to point out that due to digital implementation in computers
an acceptable estimate of joint velocities can be obtained online by numerical dif-
ferentiation, for example. Nevertheless, it is always advisable to design an observer
for this goal because even after digital implementation the system performance is
usually higher.
6.1 The Nicosia and Tomei Observer
This section reproduces one of the first joint velocity observers for robots manipu-
lators. However, the original stability proof is changed to propose a new one based
on the general methodology of this book, i.e. define a region of interest a priori
were the results are valid instead of inferring such a region as a function of different
control/observer gains.
For the robot manipulator dynamics given by (3.44), assume that no velocities
measurements are available, i.e. q̇, and the only measurable system outputs are joint
positions, i.e. q. Let q̂ be the estimated value of q and define the observation error as
z = q − q̂. (6.1)
Consider the following observer
q̂˙ = ξ + kd z (6.2)

−1 ˙ q̂˙ − Dq̂˙ − g(q) + K o z + τ ,
ξ̇ = H (q) −C(q, q̂) (6.3)
where q̂ is obtained by integrating (6.2), K o ∈ Rn×n is a positive definite diagonal

matriz and kd is a positive constant. To compute the observer error dynamics com-
bine (6.2) and (6.3) to get
https://doi.org/10.1007/978-3-030-85980-0_6
144 6 Velocity Observer Design

˙ q̂˙ + Dq̂˙ + g(q) − K o z = τ ,
H(q) q̂¨ − kd ż + C(q, q̂) (6.4)
and then subtract this relationship from (3.44) to get
˙ q̂˙ + Dż + K o z = 0.
H(q)z̈ + kd H(q)ż + C(q, q̇)q̇ − C(q, q̂) (6.5)
In view of Property 3.4 one has
˙ q̂˙ = C(q, q̇)q̇ − C(q, q̇ − ż)q̂˙

C(q, q̇)q̇ − C(q, q̂)
= C(q, q̇)q̇ − C(q, q̇)q̂˙ + C(q, ż)q̂˙
˙
= C(q, q̇)ż + C(q, ż)q̂. (6.6)
Therefore, (6.5) can be rewritten as
˙ = 0.
H(q)z̈ + C(q, q̇)ż + Dż + K o z + kd H(q)ż + C(q, q̂)ż (6.7)
To prove stability, define the observation error state for (6.7) as

z
x2 = , (6.8)
ż
and assume that the velocity vector q̇ is bounded for all t ≥ t0 by a constant vm , i.e.
q̇(t) ≤ vm ∀t ≥ t0 . (6.9)
This assumption is necessary because at this point no control law is under consid-
eration. Note that it is quite reasonable from a physical practical point of view. For
this state define a region of interest as done in (4.28)

Dz = x2 ∈ R2n \ x2 < , (6.10)
and consider the following Lyapunov candidate function
1 T 1
V (x2 ) = ż H(q)ż + zT K o z, (6.11)
2 2
which complies with (4.29), i.e.
α1 (x2 ) ≤ V (t, x2 ) ≤ α2 (x2 ),
by using
1 1
α1 (x2 ) = Lm x2 2 and α2 (x2 ) = LM x2 2 , (6.12)
2 2
6.1 The Nicosia and Tomei Observer 145
where
Lm = min{λmin (K o ), λh } and LM = max{λmax (K o ), λH }. (6.13)
λh and λH are given in Property 3.7. By using Properties 3.2 and 3.4, the derivative
of V (x2 ) along (6.7) can straightforwardly be computed as
1
V̇ (x2 ) = żT H(q)z̈ + żT Ḣ(q)ż + żT K o z
2
˙ + żT K o z
= −żT Dż − żT K o z − żT kd H(q)ż − żT C(q, q̂)ż

= −żT D + kd H(q) + C(q, q̂) ˙ ż
= −żT (D + kd H(q) + C(q, q̇) − C(q, ż)) ż. (6.14)
By using once again Property 3.7 together with Properties 3.9 and 3.10, together
with (6.9), one gets
V̇ (x2 ) ≤ − ż2 (λmin (D) + kd λh − kc vm − kc ż)

≤ − ż2 (kd λh − kc vm − kc ż) . (6.15)
Clearly, V̇ (x2 ) is negative semidefinite as long as
kd λh
ż ≤ − vm < , (6.16)
kc
and assuming that
kc vm
kd > . (6.17)
λh
Since V̇ (x2 ) is only negative semidefinite, Theorem 4.3 can be used. In fact,
V̇ (z, ż) ≡ 0 in the set
M = {(z, ż) : ż ≡ 0}. (6.18)
Therefore, it is necessary to show that the only solution (z, ż) that can stay identically
in M is the equilibrium (z, ż) ≡ (0, 0). This is carried out by substituting M, i.e.
ż ≡ 0, in (6.7) to get
K o z = 0.
Since K o is positive definite its inverse exists and as a consequence the only solution
for the previous relationship is precisely z = 0, which shows that the origin is uni-
formly asymptotically stable. As noted in Definition 4.8, (local) asymptotic stability
implies the existence of a region of attraction. This turns out to be important because
should the initial error state x2 (t0 ) be too large, then it will not necessarily vanish
even if x2 (t0 ) ∈ Dz . As can be inferred from (6.16), setting gains larger makes
larger and thus the region of attraction should be larger as well. It can be said that the
equilibrium is semi globally uniformly asymptotically stable. To check this intuition
out, a region of attraction can be found by using (4.29), (6.12) and the fact that V̇ ≤ 0
for x2 ∈ Dz to get
1 1
Lm x2 (t)2 ≤ V (x2 (t)) ≤ V (x2 (t0 )) ≤ LM x2 (t0 )2 , (6.19)
2 2
which means that

LM
x2 (t) ≤ x2 (t0 ) < , (6.20)
Lm
where the last inequality is mandatory to get x2 (t) ∈ Dz for all t ≥ t0 . Therefore, a
region of attraction is given by

Lm
A = x2 (t0 ) ∈ R \ x2 (t0 ) <
2n
. (6.21)
LM
One of the main reasons to design a velocity observer is to employ the estimates
in the robot control. Consider the control law (5.8) by making the substitution of
actual for estimated velocities
˙
τ = H(q)q̈d + C(q, q̂)q̇ ˙
d + Dq̇d + g(q) − K d (q̂ − q̇d ) − K p e, (6.22)
where the usual error definition e = q − qd in (5.1) has been employed and qd is
assumed to be bounded with at least first and second derivatives bounded as well. It
is not difficult to compute the closed loop dynamics as
H(q)ë + C(q, q̇)ė + K D ė + K p e = K d ż + C(q, ė)ż − C(q, q̇)ż, (6.23)
˙
where K D = D + K d , C(q, q̂)q̇ d = C(q, q̇ − ż)q̇d and Property 3.4 have been used.
Note the similarity with (5.9). Define the state for the system closed loop dynam-
ics (6.7) and (6.23) as
⎡ ⎤
e
x1 ⎢ ė ⎥
x= =⎢
⎣z⎦,
⎥ (6.24)
x2
ż
for which a region of the form (4.28) can be defined as

D = x ∈ R4n \ x < . (6.25)
6.1 The Nicosia and Tomei Observer 147
Note that does not need to be the very same as in (6.10) because the state is
different and the observer gains will have to be retuned. Consider the following
Lyapunov candidate function
1 T 1 1 1
V (x) = ė H(q)ė + eT K p e + żT H(q)ż + zT K o z, (6.26)
2 2 2 2
which complies with (4.29), i.e.
α1 (x) ≤ V (t, x) ≤ α2 (x),
by using
1 1
α1 (x) = Mm x2 and α2 (x) = MM x2 , (6.27)
2 2
where
Mm = min{λmin (K p ), λmin (K o ), λh } (6.28)
and
MM = max{λmax (K p ), λmax (K o ), λH }. (6.29)
λh and λH are given in Property 3.7. By using Properties 3.2 and 3.10, the derivative
of (6.26) along (6.7) and (6.23) satisfies
1 1
V̇ (x) = ėT H(q)ë + ėT Ḣ(q)ė + ėT K p e + żT H(q)z̈ + żT Ḣ(q)ż + żT K o z
2 2
˙
= −ėT K D ė + ėT K d ż − ėT C(q, q̇d )ż − żT (D + kd H(q))ż − żT C(q, q̂)ż

≤ −ėT K d ė + ėT K d ż + ėT C(q, ė) − C(q, q̇) ż − kd żT H(q)ż

−żT C(q, q̇) − C(q, ż) ż. (6.30)
Since ė is bounded in D and one can always define q̇d bounded, there exists vm ,
defined in (6.9). After Properties 3.7 and 3.9 and the definition of the region D
in (6.25), one gets
V̇ (x) ≤ − λmin (K d )ė2 − kd λh ż2 + kc (vm + )ż2

+ λmax (K d )ėż + kc (vm + )ėż. (6.31)
To achieve asymptotic stability gains have to be tuned properly, in particular kd must

be set large enough. To get insight about how large kd must be, choose δ > 0 and
assume that
λmin (K d ) ≥ δ + 1 (6.32)
δ + kc (vm + ) + (λmax (K d ) + kc (vm + ))
1 2
kd ≥ 4
, (6.33)
λh
so that (6.31) satisfies
V̇ (x) ≤ − δė2 − δż2

1
− ė2 + (λmax (K d ) + kc (vm + )) ėż − (λmax (K d ) + kc (vm + ))2 ż2
4
2
1
= − δė2 − δż2 − ė − (λmax (K d ) + kc (vm + )) ż
2
≤ − δė2 − δż2 . (6.34)
Once again V̇ (x) is only negative semidefinite and Theorem 4.3 can be used by
considering that V̇ (e, ė, z, ż) ≡ 0 only in the set
M = {(e, ė, z, ż) : ė ≡ ż ≡ 0}. (6.35)
To show that the only solution (e, ė, z, ż) that can stay identically in M is the equi-
librium (e, ė, z, ż) ≡ (0, 0, 0, 0), substitute M in (6.7) and (6.23) to get
K o z = 0 and K p e = 0.
Since K o and K p are positive definite matrices the only solution for the previous
relationships to hold are z = 0 and e = 0, which shows that the origin is uniformly
asymptotically stable. A region of attraction can be found by using (4.29), (6.27) and
the fact that V̇ ≤ 0 for x ∈ D to get
1 1
Mm x(t)2 ≤ V (x(t)) ≤ V (x(t0 )) ≤ MM x(t0 )2 , (6.36)
2 2
which means that

MM
x(t) ≤ x(t0 ) < , (6.37)
Mm
where the last inequality is mandatory to get x(t) ∈ D for all t ≥ t0 . Therefore, a
region of attraction is given by

Mm
A = x(t0 ) ∈ R4n \ x(t0 ) < . (6.38)
MM
6.2 Non Model Based Observer Design 149
6.2 Non Model Based Observer Design
The scheme proposed by Nicosia and Tomei needs the whole robot model both for
the controller and the observer, where the former is the PD+ scheme given by (5.8).
However, employing controller (5.51), i.e.
τ n = H(q)q̈r + C(q, q̇)q̇r + Dq̇r + g(q) − K d s,
can avoid using the robot model for the observer, where the usual error definition (5.1),
together with (5.49) and (5.50), i.e.
q̇r = q̇d − e
s = q̇ − q̇r = ė + e
are employed. On the other hand, consider once again the observation error given
in (6.1) and the following observer
ξ̇ = z (6.39)
q̂˙ = q̇r + kd (z + z ξ ) + z z, (6.40)
where kd is a positive real constant, z ∈ Rn×n is a diagonal positive definite matrix,

˙ Instead of (5.51), it is proposed
and q̂ is obtained by integrating q̂.
τ = H(q)q̂¨ r + C(q, q̇r )q̇r + Dq̇r + g(q) − K d so , (6.41)
with

q̂¨ r = q̈d − q̇o − q̇d (6.42)
q̇ = q̂˙ − z z
o (6.43)
so = q̇o − q̇r . (6.44)
In the same fashion as done for s, define the sliding variable
r = q̇ − q̇o = ż + z z, (6.45)
so that as before if r is bounded and tends to zero, then necessarily ż, z → 0. Note that
by taking into account (5.49)–(5.50) and (6.45), (6.42) and (6.44) can be rewritten as
q̂¨ r = q̈r + r (6.46)

so = s − r. (6.47)
Now, by using Property 3.4 to get
C(q, q̇r )q̇r = C(q, q̇)q̇r + C(q, q̇r )q̇r − C(q, q̇)q̇r
= C(q, q̇)q̇r − C(q, s)q̇r , (6.48)
the control law (6.41) can be rewritten as
τ = τ n + H(q)r − C(q, q̇r )s + K d r. (6.49)
This means that for the variable s the closed loop dynamics is given by (compare
with (5.52))
H(q)ṡ + C(q, q̇)s + K D s = H(q)r − C(q, q̇r )s + K d r, (6.50)
with K D = D + K d . Since the observer does not depend on the robot model, the cor-
responding error dynamics can be obtained in terms of r by combining the derivative
of (6.40) with (6.39) to get
q̂¨ = q̈r + kd r + z ż, (6.51)
so that
ṙ + kd r = ṡ. (6.52)
Then (6.50) can be substituted to get
ṙ + K H r = Bs, (6.53)
where
K H = kd I − − H −1 (q)K d (6.54)

B = −H −1 (q) C(q, q̇) + K D + C(q, q̇r ) . (6.55)
The proposed state for (6.50) and (6.53) to carry out the stability analysis is

s
x= , (6.56)
r
where x = 0 is the unique equilibrium point. Following one of the main ideas of the
present book, define a region of interest of the form (4.28), i.e.

D = x ∈ R2n \ x < ,
6.2 Non Model Based Observer Design 151
for some arbitrarily large 0 < < ∞. For any x ∈ D both s and r are bounded.
As explained in Section 4.4, these two variables represent the inputs of linear fil-
ters whose outputs are respectively e and z which must be bounded for x ∈ D. In
particular, the bound for e is denoted as emax so that
e ≤emax (6.57)
holds for x ∈ D. Care should be taken! This relationship does not hold for all t ≥ t0
unless x ∈ D for all t ≥ t0 , which still has to be enforced by finding proper con-
trol/observer gains and a region of attraction. Also, it is possible to get from (5.50)
ė ≤λmax ()emax + s ≤ λmax ()emax + = evm . (6.58)
Since q̇d is bounded by assumption, i.e. q̇d ≤ vdm for some positive constant vdm
and for all time, then after (6.58) q̇ is also bounded by
q̇ ≤ vdm + evm = vmax . (6.59)
The same can be said for q̇r to get
q̇r ≤ vrm . (6.60)
Consider the following positive definite function
1 T 1
V (x) = s H(q)s + rT r, (6.61)
2 2
which satisfies (4.29), i.e.
α1 (x) ≤ V (t, x) ≤ α2 (x),
with
1 1
α1 (x) = λ1 x2 and α2 (x) = λ2 x2 , (6.62)
2 2
and
1 1
λ1 = min {λh , 1} and λ2 = max {λH , 1} . (6.63)
2 2
λh and λH are given in Property 3.7. Compute the derivative of V (x) along (6.50)
and (6.53) to get
1
V̇ (x) =sT H(q)ṡ + sT Ḣ(q)s + rT ṙ
2
= − sT K D s + sT H(q)r − sT C(q, q̇r )s + sT K d r
− rT K H r + rT Bs. (6.64)
Keeping in mind that the working region of interest is D, within it it is possible to

calculate the following bounds
β1 = kc vrm (6.65)
β2 =λH λmax () + λmax (K d )
1
+ (kc (vmax + vrm ) + λmax (K D )) (6.66)
λh
1
β3 = λmax () + λmax (K d ), (6.67)
λh
by taking into account Properties 3.7, and 3.9, (6.54), (6.55), (6.59), (6.60). Therefore,
(6.64) satisfies
V̇ (x) ≤ −(λmin (K d ) − β1 )s2 + β2 sr − (kd − β3 )r2 (6.68)
in view of Property 3.10. Choose gains so that
λmin (K d ) ≥ δ + 1 + β1 (6.69)
1
kd ≥ δ + β3 + β22 , (6.70)
4
for some δ > 0 to get
V̇ (x) ≤ − δs2 − δr2 = −δx2 ≤ 0. (6.71)
This means that Condition 2 of Corollary 4.1 is met so that the origin x = 0 of (6.50)
and (6.53) is asymptotically stable. Note that as made before a region of attraction
can be computed to be

λ1
A = x(t0 ) ∈ R 2n
\ x(t0 ) < . (6.72)
λ2
Finally, the result cannot be made global despite V (x) is radially unbounded, although
the region D can be made arbitrarily large.
6.3 Non Model Based Observer and Control Design 153
6.3 Non Model Based Observer and Control Design
As shown in the previous section, it is possible to design a velocity observer without

using the robot model. With some modifications, it is also possible to do the same
for the controller. Consider once again the observer error definition z in (6.1) and the
following two new errors
eo = q̂ − qd (6.73)
ėo = q̂˙ − q̇ .
d (6.74)
Then, define the following velocity observer
q̂˙ = ζ + z z + kd z (6.75)
ζ̇ = q̈d − ėo + kd z z, (6.76)
where as usual q̂ is obtained by integrating q̂, ˙ q is a bounded desired trajectory

d
for q, with at least first and second derivatives bounded, , z ∈ Rn×n are diagonal
positive definite matrices and kd is a positive constant. Consider now instead of the
usual definition of s in (5.50) the following one
s = q̂˙ − q̇d + (q̂ − qd ) = ėo + eo . (6.77)
Also, instead of q̇r in (5.49) define
q̇r = q̇d − eo − K γ σ , (6.78)
where K γ ∈ Rn×n is a diagonal positive definite matrix and
t

σ = K β s(ϑ) + sign(s(ϑ)) dϑ σ (t0 ) = 0, (6.79)
0
where K β ∈ Rn×n is a diagonal positive definite matrix and

T
sign(s) = sign(s1 ) · · · sign(sn ) , (6.80)
with si element of s, i = 1, . . . , n. Alternatively to (6.79) one can use
d
σ = K β s + sign(s). (6.81)
dt
Finally, use again (6.43) and (6.44), i.e.
q̇o = q̂˙ − z z
so = q̇o − q̇r ,
to define the control law
τ = −K p so , (6.82)
with K p ∈ Rn×n a positive definite matrix. It is possible to show that gains can always
be found to achieve e, ė, z, ż → 0, but before carrying out a mathematical analysis,
let get some insight about what is the basis of such a simple control law. Assume that
the observation errors are zero, meaning that q̂ = q and q̂˙ = q̇ and therefore q̇o = q̇,
ėo = ė, eo = e and the usual definition s = ė + e is recovered. In this case, the
control law (6.82) can be written as

τ = −K p ė + e + K γ σ . (6.83)
In view of the definition of σ in (6.79), then it can be written
τ = − K p e − K p K γ σ − K p ė , (6.84)

P I D
which shows that the control law is a nonlinear PID but with the employment of an
observer and the user can tuned gains according to his/her own experience. Before
carrying out the stability proof, the following lemma is presented.
Lemma 6.1 Consider (6.79)–(6.81), and assume that it is possible to form the fol-
lowing relationship
u = s + Kγ σ , (6.85)
for some bounded input u, i.e. u ≤ ū < ∞ for all t ≥ t0 . Then σ and s are bounded
for all time.
Proof Define V (t, σ ) = 21 σ T σ , so that (4.29) holds with
1
α1 (σ ) = α2 (σ ) = σ 2 . (6.86)
2
Compute the derivative of V (t, σ ) using (6.81) to get by using (6.85)
d
V̇ = σ T σ = σ T K β s + σ T sign(s)
dt
= −σ T K β K γ σ + σ T K β u + σ T sign(s). (6.87)
√
But sign(s) = sign(s1 )2 + · · · + sign(sn )2 ≤ n, so that it holds
√
V̇ ≤ − λmin (K β K γ )σ 2 + σ λmax (K β )ū + n

1 1 √
≤ − λmin (K β K γ )σ − σ λmin (K β K γ )σ − λmax (K β )ū + n .
2
2 2
(6.88)
Then, if
√
2 λmax (K β )ū + n
σ ≥ μ = , (6.89)
λmin (K β K γ )
one has
1
V̇ ≤ − λmin (K β K γ )σ 2 = −W3 (σ ). (6.90)
2
Equations (6.87)–(6.90) hold for D = Rn , and therefore a positive value can always
be found to satisfy (4.55). Furthermore, since σ (t0 ) = 0 as given in (6.79), it is easy to
show that σ ≤ μ = σmax . Finally, since σ is bounded, from (6.85) s must be bounded
as well.
To prove that both tracking and observation errors tend to zero, use r given
in (6.45), i.e.
r = q̇ − q̇o = ż + z z,
and define based on (6.73) and (6.78) and
sr = q̇ − q̇r = ė + e − z + K γ σ . (6.91)
Then, one can rewrite (3.44) as
H(q)(ṡr + q̈r ) + C(q, q̇)(sr + q̇r ) + D(sr + q̇r ) + g(q) = τ , (6.92)
or
H(q)ṡr + C(q, q̇)sr + Dsr = τ − ya , (6.93)
where
ya = H(q)q̈r + C(q, q̇)q̇r + Dq̇r + g(q). (6.94)

Note that as before so can be rewritten as so = sr − r, so that (6.82) and (6.93) become
τ = −K p sr + K p r, (6.95)
and
H(q)ṡr + C(q, q̇)sr + K Dp sr = K p r − ya , (6.96)
respectively, where K Dp = D + K p . As to the observer error dynamics, first of all

note that (6.75) can be written as
q̂¨ = ζ̇ + z ż + kd ż, (6.97)
so that by substituting in (6.76) one gets
q̂¨ = q̈d − ėo + kd z z + z ż + kd ż

= q̈d − ėo + z ż + kd r. (6.98)
Equivalently one has
d d
q̈ − q̈d + ėo + K γ σ = q̈ − q̂¨ + z ż + kd r + K γ σ , (6.99)
dt dt
so that from (6.43), (6.45), (6.78) and (6.91) it is
d
ṙ + kd r = ṡr − K γ σ. (6.100)
dt
Define

sr
x= (6.101)
r
as state for (6.96) and (6.100) and choose for this state a region D similar to that
given in (4.28), i.e.

D = x ∈ R2n \ x ≤ b ,
for some arbitrarily large 0 < b < ∞. Although the notation could have been
employed as usual, this time it is not asymptotic stability but ultimate boundedness
of the state what it is sought for the reasons that will be explained later, so that the
notation complies with Definition 4.9.1. Note also that ≤ is used instead of <. The
procedure can be divided in three steps:
(a) First of all, it will be shown that if x ∈ D, i.e. x ≤ b, then any other signal or
variable of interest is bounded. Care should be taken! This does not mean that it
is assumed that x is bounded for all time, this has to be eventually proven, but it
means that it is necessary to find out what happens if x ∈ D. For example, one
could have z = q − q̂ ≡ 0, but if q is not bounded then neither q̂ is bounded.
Otherwise said, the boundedness of the state does not imply the boundedness of
all other variables and this has to be shown prior to look for gains to guarantee
ultimate boundedness. This proceeds as follows. From (6.77) and (6.78) one gets
q̇r = q̂˙ − s − K γ σ . (6.102)
Hence
sr = q̇ − q̇r = r + s + K γ σ − z z. (6.103)
Choose u = sr − r + z z to have an equation of the form (6.85). Recall that

after (6.45) both z and ż are bounded if r is bounded, and in view of (6.101) both
sr and r are bounded in D. Thus, u is bounded and so are σ and s, according to
Lemma 6.1. On the other hand, from (6.91) one has
ė + e = sr + z − K γ σ , (6.104)
which is a stable linear filter with bounded input, so that e and ė must be bounded.
But, since e = q − qd , and qd and its derivatives are bounded by assumption, it
can be concluded that q and q̇ are also bounded. On the other hand, from (6.78)
one gets directly
d
q̈r = q̈d − ė + ż − K γ σ, (6.105)
dt
which must be bounded because from (6.81) it is known that dtd σ is bounded.
In turn, this means that ya in (6.94) is bounded. Thus, both ṡr in (6.96) and ṙ
in (6.100) must be bounded in D. Finally, note that u̇ = ṡr − ṙ + z ż is then
bounded.
(b) The next step is to show that, with a proper choice of gains, one can actually
achieve x ≤ b, whenever qd and at least its first and second derivatives are
bounded and x(t0 ) ≤ a for a constant a to be found. Note that Definition 4.9.1
states that b = b(a), i.e. the proposed value should be a, not b. But this is of little
relevance because Theorem 4.5 is to be used. Define
1 T 1
V (x) = sr H(q)sr + rT r, (6.106)
2 2
for which (4.29) holds with
α1 (x) = λ1 x2 and α2 (x) = λ2 x2 , (6.107)
where
1 1
λ1 = min {λh , 1} and λ2 = max {λH , 1} . (6.108)
2 2
As usual, Property 3.7 has been employed. Also, by using Property 3.2, the
derivative of V (x) along (6.96) and (6.100) is given by
V̇ = −sTr K Dp sr + sTr K P r − sTr ya

d
− kd rT r + rT ṡr − rT K γ σ . (6.109)
dt
While V (x) is positive definite for all x ∈ R2n , its derivative cannot be negative
definite or at least negative semidefinite for all x ∈ R2n . What is sought is to
satisfy condition (4.53) in D to apply Theorem 4.5. In this region, as shown in
item a), bounds can always be found so that it holds
V̇ ≤ −λmin (K P )sr 2 + λmax (K P )sr r − kd r2 + cs sr + cr r,

(6.110)
where Property 3.10 has been used and
ya ≤ cs (6.111)
ṡr ≤ spmax (6.112)

d
σ ≤ σpmax (6.113)
dt
cr = spmax + λmax (K γ )σpmax (6.114)
are bounds only valid whenever x ∈ D. Therefore, if gains are chosen to satisfy
cs + cr
λmin (K p ) ≥ δ + 1 + (6.115)
μ
1 cs + cr
kd ≥ δ + λmax (K P )2 + , (6.116)
4 μ
for two positive constant δ and μ, then one gets
V̇ ≤ − δsr 2 − δr2
1
− sr 2 + λmax (K P )sr r − λmax (K P )2 r2
4
cs + cr cs + cr
− sr 2 − r2 + cs sr + cr r
μ μ
2
1
≤ − δx − sr − λmax (K P )r
2
2
cs + cr
− x + (cs + cr )x.
2
(6.117)
μ
Therefore it is
cs + cr
V̇ ≤ −δx2 − x (x − μ) , (6.118)
μ
so that if x ≥ μ > 0 then condition (4.53) holds with
V̇ ≤ −δx2 = −W3 (x). (6.119)
To apply Theorem 4.5 it is only necessary to find so that (4.55) is satisfied, i.e.

λ1
μ< . (6.120)
λ2
This can be done just by choosing = b and noting that μ is an arbitrary control
parameter so that this goal can always be achieved by setting it small enough.
Finally, one only has to assume that condition (4.56) on x(t0 ) is fulfilled.
(c) Till now it has only been shown that x is uniformly bounded. Although ultimate
uniform boundedness can also be proven, the main goal is to show that tracking
and observation errors tend to zero. To do this, from (6.81) and (6.103) one
obtains
ṡ = −K γ K β s − K γ sign(s) + u̇. (6.121)
Note that u̇ = ṡr − ṙ + z ż, so that it must be bounded according to the discus-
sion of Step a). Now choose V1 = 21 sT s = 21 s2 to get
ds
s ≤ −λmin (K γ K β )s2 − λmin (K γ )|s| + supmax , (6.122)
dt
where upmax is a bound for u̇ by considering x ∈ D and |s| = |s1 | + · · · + |sn |,

with si element of s, i = 1, . . . , n. Since s ≤ |s|, an equation of the form (4.79)
can be gotten
1 d ds
s2 = s ≤ − λmin (K γ ) − upmax s, (6.123)
2 dt dt
by setting η = λmin (K γ ) − upmax > 0. This can be done by choosing λmin (K γ )
large enough. Therefore, there exists a finite time tr so that s(t) ≡ 0 for all
t ≥ tr ≥ t0 according to the discussion of Section 4.4. This means that from
t ≥ tr one has s(t) = 0, so that (6.77) becomes
ėo + eo ≡ 0. (6.124)
Since
ėo + eo = ė + e − ż − z = 0, (6.125)
equation (6.91) can be rewritten as
sr = ėo + eo + ż + z − z + K γ σ = r − z z + K γ σ . (6.126)
Compute its derivative to get
d
ṡr − ṙ = −z ż + K γ σ. (6.127)
dt
But from (6.100) it is also
d
ṡr − ṙ = kd r + K γ σ. (6.128)
dt
Then, from (6.127)–(6.128) one has
(kd I + z )ż + kd z z = 0. (6.129)
Equation (6.129) represents a stable linear filter for z, so that z and ż tend to zero.
This in turn means from (6.125) that e and ė do tend to zero as well.
6.4 Experimental Results 161
6.4 Experimental Results
In this section, a set of experiments is presented to show the performance of the above
observers. For this purpose, the Phantom Omni device described in Chapter 14 is
employed. For the experiments below, only the first three degrees of freedom are used,
while the remaining three are mechanically blocked, as explained in Sect. 14.2. The
dynamic model (14.3) along with the parameters shown in Sect. 14.2.5 are employed.
The experiments presented below are:
1. The Nicosia-Tomei (NT) observer (6.2)–(6.3) along with the control law (6.22)
2. The non-model based observer (NMO) in (6.39)-(6.40) with the control law (6.41)
3. The non model based observer and controller (NMOC) given in (6.75)–(6.76)
and (6.82)
For all the experiments, the desired joint trajectories are
qdi (t) = ai + bi (1 − cos(ωd t)) , (6.130)
where (a1 , a2 , a3 ) = (−30◦ , 20◦ , −60◦ ), (b1 , b2 , b3 ) = (30◦ , 30◦ , −30◦ ), and ωd =
π . The gains chosen for the experiments are shown in Table 6.1.
Figures 6.1 and 6.2 show the tracking behavior. It is important to recall that this is
the main objective and it can be concluded that the three control-observer approaches
deliver similar outcomes. Regarding the observers performances, they are difficult
to evaluate because the actual velocity q̇ is not known. Therefore, Fig. 6.3 shows
the estimated values vs the desired ones, while Fig. 6.4 shows the corresponding
errors. Finally, the input torques are shown in Fig. 6.5. Certainly, one could have
expected the worst behavior for the NT scheme because of its complete dependence
on the robot model, but as a matter of fact the discretization used to implement
the different algorithms plays an important role. Therefore, even a relatively small
sampling time can largely increase the error in calculating the integral term of the
NMOC, which explains why a zero tracking or observation error is not achieved as
foreseen theoretically. Still, perhaps this is the best option simple because it does not
need any robot model at all and it is easier to program.
Table 6.1 Gains chosen for the experiments

Scheme Observers’ gains Controllers’ gains
NT kd = 100, K o = 0.001I K p = 4I, K d = 0.08I
NMO kd = 100, z = I K d = 0.1I, = 25I
NMOC kd = 100, z = I, = 50I K β = I, K γ = I, K o = 0.1I
Fig. 6.1 Joint position tracking: desired (· · · ), NT (- . -), NMO (—), NMOC (– –)
Fig. 6.2 Joint position error: NT (- . -), NMO (—), NMOC (– –)

Fig. 6.3 Joint velocities estimation and tracking: desired (· · · ), NT (- . -), NMO (—), NMOC (– –)
Fig. 6.4 Joint velocities error: NT (- . -), NMO (—), NMOC (– –)

Fig. 6.5 Input torques: NT (- . -), NMO (—), NMOC (– –)
References
Arteaga-Pérez MA, Castillo-Sánchez AM, Parra-Vega V (2006) Cartesian control of robots without
dynamic model and observer design. Automatica 42:473–480
Arteaga-Pérez MA, Ortiz-Espinoza A, Romero JG, Espinoza-Pérez G (2020) On the adaptive control
of robot manipulators with velocity observers. Int J Rob Nonlinear Control 30:4371–4396
Nicosia S, Tomei P (1990) Robot control by using only joint position measurements. IEEE Trans
Autom Control 35(9):1058–1061
Chapter 7
Adaptive and Robust Control
Abstract In the previous two chapters it was shown that the control of robot manip-
ulators is quite direct despite the high nonlinear nature of these systems as long as
the complete robot model is available. It was also shown that some techniques based
on sliding mode control theory can be employed to overcome the complete lack of
the robot analytical model and even the lack of velocity measurements. A possible
drawback of these control laws is that the discretization process for computer imple-
mentation tends to diminish the overall system performance. This chapter presents
other well known control techniques employed to deal with a poor knowledge of the
robot model parameters.
7.1 The Adaptive Law by Slotine and Li
Most of the control laws studied in Chaps. 5 and 6 rely on the exact knowledge of the
robot manipulators. However, in practice there is usually some parameter uncertainty
that might just decrease performance. For example, consider the nominal control law
given by (5.51), i.e.
τ n = H(q)q̈r + C(q, q̇)q̇r + Dq̇r + g(q) − K d s,
where (5.1), (5.49) and (5.50) are used, i.e.
e = q − qd
q̇r = q̇d − e
s = q̇ − q̇r = ė + e,
and K d , ∈ Rn×n are positive definite diagonal matrices. However, the actual con-
troller is rather given by
τ = Ĥ(q)q̈r + Ĉ(q, q̇)q̇r + D̂q̇r + ĝ(q) − K d s, (7.1)
https://doi.org/10.1007/978-3-030-85980-0_7
166 7 Adaptive and Robust Control
where (ˆ·) means that estimated values instead of the actual model parameters are
being used. At this point it is worthy to point out that implicitly it is assumed that the
robot model structure is well known and what is uncertain are constant parameters
just as moments of inertia, link masses or lengths, for example. This is reasonable
because the robot kinematic is usually well known, but masses and center of masses
are hard to compute exactly due to a variety of factors, like the links shapes and
non uniform masses densities. In the last part of the book the computation of many
industrial robots is described, but for the moment let’s take advantage of Property 3.14
to rewrite (7.1) as
τ = Ĥ(q)q̈r + Ĉ(q, q̇)q̇r + D̂q̇r + ĝ(q) − K d s

= Y(q, q̇, q̇r , q̈r )ϕ̂ − K d s, (7.2)
where ϕ̂ represents an estimated or nominal value of the actual vector of parameters

ϕ. An acceptable value of ϕ̂ can provide a good control performance and nothing
else is advisable to do, but if uncertainties are not negligible, then it may be necessary
to increase performance by somehow compensating them explicitly. Two common
options are adaptive control and Lyapunov redesign. The former estimates ϕ̂ online
until the performance is adequate and the latter adds a second term to the nominal
control law meant to deal with uncertainties and perturbations. It is also possible
to combine both techniques. One of the best known adaptive approaches is the one
introduced by Slotine and Li. For simplicity define
Y a = Y(q, q̇, q̇r , q̈r ), (7.3)
so that (7.2) can be written as
τ = Ĥ(q)q̈r + Ĉ(q, q̇)q̇r + D̂q̇r + ĝ(q) − K d s = Y a ϕ̂ − K d s. (7.4)
In fact, the nominal control law can be written in the very same fashion as
τ n = H(q)q̈r + C(q, q̇)q̇r + Dq̇r + g(q) − K d s = Y a ϕ − K d s, (7.5)
where it can clearly be appreciated that the difference between the actual and the
nominal laws lies in the parameter vector. Add and subtract Y a ϕ from (7.4) to get
τ = Y a ϕ̂ − K d s + Y a ϕ − Y a ϕ

= Y a ϕ − K d s + Y a ϕ̂ − ϕ
= τ n + Y a ϕ̃, (7.6)
where
ϕ̃ = ϕ̂ − ϕ (7.7)
7.1 The Adaptive Law by Slotine and Li 167
is the parameter error. Therefore, by substituting (7.6) in the robot model (3.44) the
closed loop error dynamics can be straightforwardly computed as
H(q)ṡ + C(q, q̇)s + K d s = Y a ϕ̃, (7.8)
where K D = D + K d . Also this equation allows to see that if ϕ̃ ≡ 0 then s → 0 and

e, ė → 0 as shown in Sect. 5.4. Assume now that ϕ̂ varies so that a vector error state
x ∈ Rn+p can be introduced as

s
x= , (7.9)
ϕ̃
although ϕ̃˙ is still undefined. Consider the following Lyapunov candidate function
for s and ϕ̃
1 T 1
V (x) = s H(q)s + ϕ̃ T −1 ϕ̃, (7.10)
2 2
where ∈ Rp×p is a diagonal positive definite matrix. The derivative of V (x)

along (7.8) is given by
1
V̇ (x) = sT H(q)ṡ + sT Ḣ(q)s + ϕ̃˙ T −1 ϕ̃
2
= −sT K D s + sT Y a ϕ̃ + ϕ̃˙ T −1 ϕ̃. (7.11)
As usual, Property 3.2 has been employed. Note that ϕ is a constant vector and
therefore from (7.7) one gets
˙
ϕ̃˙ = ϕ̂. (7.12)
Choosing ϕ̂˙ implies the design of an adaptive law. The most obvious choice is
ϕ̃˙ = ϕ̂˙ = −Y Ta s, (7.13)
so that (7.11) becomes
V̇ (x) = −sT K D s. (7.14)
Clearly, V̇ (x) is negative semidefinite and V̇ (x) ≡ 0 only in the set
M = {(s, ϕ̃) : s ≡ 0}, (7.15)

so that it is tempting to use Theorem 4.3. However, substituting s ≡ 0 in (7.8) leads to
Y a ϕ̃ = 0. (7.16)
Unfortunately, x ≡ 0 is not the only solution x that can stay identically in M since
ϕ̃ may happen to lie in the null space of Y a and Theorem 4.3 cannot be employed.
This does not only imply that the equilibrium x ≡ 0 of (7.8) and (7.13) cannot be
shown to be asymptotically stable, but in fact it cannot even be shown that
lim s = 0. (7.17)
t→∞
The fact that V̇ (x) ≤ 0 while V (x) is lower bounded by 0 implies that x is bounded
for all t ≥ t0 and that V (x) tends to a constant value. Technically, this does not mean
that V̇ (x) → 0 and therefore (7.17) does not hold necessarily. However, this can be
proven by using Barbalat’s Lemma 4.5, for which it only has to be shown that V̈ (x)
is bounded. By direct computation it is
V̈ (x) = −2sT K D ṡ
= 2sT K D H −1 (q) (C(q, q̇)s + K D s − Y a ϕ̃) , (7.18)
which shows that V̈ (x) is bounded and as stated by Barbalat’s Lemma V̇ (x) → 0 and
as consequence (7.17) holds, so that in the end e and ė tend to zero as well, which is
the main control objective.
A natural question is whether it is also possible to have ϕ̃ → 0. In fact, it can
be shown that if the regressor Y a is persistently exciting, i.e. if there exist positive
constants T1 and α1 such that for all t ≥ t0 it holds
1
t+T
Y Ta (t)Y a (t)dt ≥ α1 I, (7.19)

t
then additionally to e, ė → 0 it is also ϕ̃ → 0. A formal technical proof is out of the

scope of this book but it can be found in the references provided at the end of this
chapter.
7.2 Adaptive Scheme with Velocity Observers
Assume now that joint velocities are not available. For that case the use of an observer
for velocity estimation is advisable. However, the problem becomes more involved
if the estimated parameters are used both for the observer and the controller. A
possibility is to employ the observer introduced in Sect. 6.2 in Eqs. (6.1) and (6.39)–
(6.40), i.e.
7.2 Adaptive Scheme with Velocity Observers 169
z = q − q̂
ξ̇ = z
q̂˙ = q̇r + kd (z + z ξ ) + z z,
where kd is a positive real constant, z ∈ Rn×n is a diagonal positive definite matrix,

˙ For that case the control law (6.41) becomes
and q̂ is obtained by integrating q̂.
τ = Ĥ(q)q̂¨ r + Ĉ(q, q̇r )q̇r + D̂q̇r + ĝ(q) − K d so , (7.20)
where (6.42)–(6.44) have been employed, i.e.

q̂¨ r = q̈d − q̇o − q̇d
q̇ = q̂˙ − z z
o
so = q̇o − q̇r .
Note the similarity of (7.1) with (7.20). The latter can be rewritten as
τ = H(q)q̂¨ r + C(q, q̇r )q̇r + Dq̇r + g(q) − K d so + Y a ϕ̃, (7.21)
where the parameter error defined in (7.7) is used and
Y a ϕ̃ = Y(q, q̇r , q̂¨ r )ϕ̃ = H̃(q)q̂¨ r + C̃(q, q̇r )q̇r + D̃q̇r + g̃(q). (7.22)
After (6.49), i.e.
τ o = τ n + H(q)r − C(q, q̇r )s + K d r,
Equation (7.21) can be rewritten as
τ = τ n + H(q)r − C(q, q̇r )s + K d r + Y a ϕ̃, (7.23)
where (6.45)–(6.47) have been used, i.e.
r = q̇ − q̇o = ż + z z
q̂¨ = q̈ + r
r r
so = s − r.
By considering (6.50), i.e.
H(q)ṡ + C(q, q̇)s + K D s = H(q)r − C(q, q̇r )s + K d r,

with K D = D + K d , the closed loop dynamics for s is given by
H(q)ṡ + C(q, q̇)s + K D s = H(q)r − C(q, q̇r )s + K d r + Y a ϕ̃, (7.24)
while instead of (6.53), i.e.
ṙ + K H r = Bs,
one has
ṙ + K H r = Bs + H −1 (q)Y a ϕ̃, (7.25)
where (6.54) and (6.55) are considered, i.e.
K H = kd I − − H −1 (q)K d

B = −H −1 (q) C(q, q̇) + K D + C(q, q̇r ) .
It is easy to note that the only differences between (6.50) and (7.24) and
between (6.53) and (7.25) are the terms Y a ϕ̃ and H −1 (q)Y a ϕ̃, respectively. In
Sect. 6.2 it was shown that for ϕ̃ ≡ 0 then s, r → 0 and consequently e, ė, z, ż → 0.
Intuitively in general this will not be the case, but also intuitively if ϕ̃ ≤ ϕmax < ∞
∀ t ≥ t0 , the tracking and observation errors will not tend to zero but will remain
ultimately bounded. To check this intuition out and to find out whether the ultimate
bound can be made arbitrarily small or not, consider the following state defition

s
y= , (7.26)
r
for (7.24) and (7.25). Note that this definition excludes ϕ̃ as part of the state, but
this is on purpose to treat it rather as a perturbation. Note also that ϕ̃ ≤ ϕmax < ∞
∀ t ≥ t0 can be achieved simply by considering ϕ̂ to be constant or by design of
an adaptive law specially for this goal. Then consider once again a region Dy as
in (4.28), say

Dy = y ∈ R2n \ y < ,
for some arbitrarily large 0 < < ∞. With this background Theorem 4.5 can be
employed by using
1 T 1
V (y) = s H(q)s + rT r, (7.27)
2 2
α1 (y) ≤ V (t, y) ≤ α2 (y),
by considering (6.62) and (6.63), i.e.
1 1
α1 (y) = λ1 y2 and α2 (y) = λ2 y2 ,
2 2
and
λ1 = min {λH , 1} and λ2 = max {λH , 1} ,
where λh and λh are given in Property 3.7. To show that condition (4.53) also holds
for some μ > 0, it is not difficult to get from (6.64)–(6.71) that the derivative of V (y)
along (7.24)–(7.25) satisfies

V̇ (y) ≤ −δy2 + sT + rT H −1 (q) Y a ϕ̃. (7.28)
Note that from (6.46) the boundedness of q̂¨ r in Dy can be concluded by considering
that the derivative of q̇r = q̇d − e must be bounded in Dy . This means that there
exists some yam > 0 so that Y a ≤ yam < ∞ for y ∈ Dy . Therefore, (7.28) satisfies
1
V̇ (y) ≤ − δy2 + y 1 + yam ϕmax
λh
1
= − δy2
2
1 1
− y δy − 1 + yam ϕmax , (7.29)
2 λh
where λH is given in Property 3.7. Clearly, whenever
2 1
y ≥ 1+ yam ϕmax = μ, (7.30)
δ λh
it holds
V̇ (y) ≤ −δy2 = −W3 (y). (7.31)
To comply with condition (4.55), i.e.
μ < α2−1 (α1 ()),

one must have that
λ1
μ< , (7.32)
λ2
which will be satisfied as long as
2 λ2 1
1+ yam ϕmax < δ, (7.33)
λ1 λh
for δ > 0. Furthermore, note that (4.56), i.e.
y(t0 ) = y0 ≤ α2−1 (α1 ())
actually complies with the region A given in (6.72), i.e.

λ1
A = y(t0 ) ∈ R2n \ y(t0 ) < .
λ2
Therefore, according to Theorem 4.5 there exists a finite 0 ≤ T , so that from t0 + T ≤

t the state y will be ultimately bounded by (see (4.73))
λ2
y < μ = bf . (7.34)
λ1
But this means that also from t0 + T ≤ t then it holds s ≤ bf and r ≤ bf which
necessarily mean that e, ė, z and ż can be made arbitrarily small by setting δ arbitrarily
large to make μ, and therefore bf , arbitrarily small.
The previous analysis may appear to be unnecessary but it allows to show one
of the main problems to be solved when using parameter and velocity estimation
together. Complete the state y in (7.26) to include the parameter error ϕ̃, i e.
⎡ ⎤
s
y
x= = ⎣ r ⎦, (7.35)
ϕ̃
ϕ̃
so that additionally to the positive definite function V (y) in (7.27) it can be included
the term
1 T −1
Vϕ (ϕ̃) = ϕ̃ ϕ̃, (7.36)
2
to get
1
V (x) = V (y) + ϕ̃ T −1 ϕ̃, (7.37)
2
whose derivative according to (7.28) satisfies

V̇ (x) ≤ −δy2 + sT + rT H −1 (q) Y a ϕ̃ + ϕ̃˙ T −1 ϕ̃. (7.38)
Therefore, to get the same result as in the previous section, the adaptive law would
have to be

ϕ̃˙ = −Y Ta s + H −1 (q)r , (7.39)
where it has been taken into account that H −1 (q) is symmetric. Clearly, (7.39) is
not implementable. A possibility lies in designing a composite algorithm, which are
those adaptive laws that use joint tracking errors and parameter prediction errors
together. To form the latter it is necessary to get the filtered input τ f ∈ Rn as
d
τ f = −λf τ f + λf τ = −λf τ f + λf Y(q, q̇, q̈)ϕ, (7.40)
dt
with λf > 0 and where Property 3.14 is employed. The corresponding filtered regres-
sor Y f (q, q̇) ∈ Rn×p can then be computed in the same fashion as
d
Y f = −λf Y f + λf Y(q, q̇, q̈), (7.41)
dt
resulting in
t
Y f (q, q̇) = λf e−λf (t−ϑ) Y(ϑ)dϑ, (7.42)
0
where the arguments q, q̇ and q̈ have been omitted for simplicity and zero initial
conditions are chosen. Note that the effect of filtering the regressor is to eliminate
the acceleration q̈ and that since the vector of parameters is constant, then it holds
τ f = Y f (q, q̇)ϕ. (7.43)
Finally, the prediction error is defined as:
= Y f ϕ̂ − τ f = Y f ϕ̃, (7.44)
with ∈ Rn . However, since q̇ is not available, an estimated filtered regressor is

defined as
Ŷ f (q, q̇o ) = Y f (q, q̇o ), (7.45)
with Ŷ f (q, q̇o ) ∈ Rn×p . For simplicity’s sake, Y f or Y f (·) and Ŷ f or Ŷ f (·) will be used
hereafter as long as there is no place for confusion, while the prediction error has to
be redefined as
= Ŷ f ϕ̂ − τ f = Ŷ f ϕ̂ − Y f ϕ, (7.46)
This allows to propose the following adaptive law

T
˙
ϕ̂(t) = − β Ŷ f + Y Ta so , (7.47)
where ∈ Rp×p is a diagonal positive definite gain matrix and β is a constant scalar
gain. Define
Ỹ f (t) = Ŷ f (q, q̇o ) − Y f (q, q̇) (7.48)
with Ỹ f (t) ∈ Rn×p , so that by using (7.46) one has

T T T T T
Ŷ f = Ŷ f Ŷ f ϕ̂ − Ŷ f Y f ϕ = Ŷ f Ŷ f ϕ̃ + Ŷ f Ỹ f ϕ. (7.49)
This means that (7.47) is equivalent to

T
˙
ϕ̂(t) ˙
= ϕ̃(t)
T
= − β Ŷ f Ŷ f ϕ̃ + β Ŷ f Ỹ f ϕ + Y Ta so . (7.50)
Assume now that Ŷ f is persistently exciting, i.e. that

t+Tf
T
Ŷ f (t)Ŷ f (t)dt ≥ αf I
t
holds for some positive constants Tf and αf for all t ≥ t0 . Then, as pointed out in
many of the references at the end of this chapter, the autonomous system
˙
ϕ̃(t)
T
= −β Ŷ f Ŷ f ϕ̃ (7.51)
is exponentially stable. Therefore, converse theorems guarantee that there exists a

positive definite function Vϕ (ϕ̃) satisfying (4.29) in the form
c1 ϕ̃2 ≤ Vϕ (ϕ̃) ≤ c2 ϕ̃2 ,

and (4.40) in the form
V̇ϕ (ϕ̃) ≤ −c3 ϕ̃2 ,
for some positive constants c1 , c2 and c3 . However, knowing that these conditions are
satisfied and finding the corresponding Vϕ (ϕ̃) are different things. For example, the
positive definite function given in (7.36) clearly satisfies (4.29). With this in mind
V (x) in (7.37) satisfies (4.29) with
1 1
α1 (y) = λ1 y2 and α2 (y) = λ2 y2 ,
2 2
and
1 1
λ1 = min λh , 1, and λ2 = max λH , 1, ,
λmax () λmin ()
where λh and λH are given in Property 3.7. To get a stability result, define as usual a
region of interest of the form (4.28), i.e.

D = x ∈ R2n+p \ x < ,
for some 0 < < ∞ which does not need to be the same as in Dy . In this region it
is not difficult to show that
Ỹ f (q, q̇, q̇o ) ≤ yfm r (7.52)
holds for some 0 < yfm < ∞. Therefore, according to (7.28) and (7.50), V̇ (x) satis-
fies

V̇ (x) ≤ − δy2 + sT + rT H −1 (q) Y a ϕ̃
T T
− β ϕ̃ T Ŷ f Ŷ f ϕ̃ − β ϕ̃ T Ŷ f Ỹ f ϕ − ϕ̃ T Y Ta (s − r)

= − δy2 + rT I + H −1 (q) Y a ϕ̃
T T
− β ϕ̃ T Ŷ f Ŷ f ϕ̃ − β ϕ̃ T Ŷ f Ỹ f ϕ. (7.53)
T
At first sight it can be thought that c3 = βλmin (Ŷ f Ŷ f ). However, Ŷ f ∈ Rn×p and in
T
general n < p so that at most rank(Ŷ f ) = n and thus λmin (Ŷ f Ŷ f ) = 0. Therefore,
another Vϕ (ϕ̃) for which c3 > 0 must be found so that a combination of gains to get
x → 0 can be found. This positive definite function must exist if Ŷ f is persistently
exciting as said before, but it appears to be a waste of time to look for it because
in the end Ŷ f satisfying this condition is only an assumption which may be hard to
enforce. On the contrary, consider the more reasonable following assumption.
Assumption 7.1 For each element ϕi of the parameter ϕ with i = 1, . . . , p, lower

and upper bounds, ϕmi and ϕMi , are known which satisfy
ϕmi ≤ ϕi ≤ ϕMi . (7.54)
This assumption is more relaxed than it appears to be because at least the sign of
the parameter ϕi is always known (say it is positive), so that the lower bound is 0. The
upper bound can be considered any constant value arbitrarily large. Then, instead
of (7.47) consider
T
˙
ϕ̂(t) = − β Ŷ f + Y Ta so + f b ϕmi ≤ ϕ̂i (0) ≤ ϕMi , (7.55)
where for i = 1, . . . , p, each element of f b ∈ Rp is given by

⎧
⎪
⎪ ϕ − ϕ̂ T
⎪ − ϕ 2i − ϕ i ρi |β Ŷ fi + yTai so | if ϕ1i ≤ ϕ̂i < ϕ2i
⎪
⎪ 2i 1i
⎪
⎨
fbi = 0 if ϕ2i ≤ ϕ̂i ≤ ϕ3i , (7.56)
⎪
⎪
⎪
⎪
⎪
⎪
⎩ ϕ̂i − ϕ3i ρ |β Ŷ T + yT s | if ϕ < ϕ̂ ≤ ϕ
ϕ4i − ϕ3i i fi ai o 3i i 4i
T T
where ρi > 1, Ŷ fi is the ith row of Ŷ f , yTai is the ith row of Y Ta , and ϕ1i < ϕ2i ≤ ϕmi <
ϕMi ≤ ϕ3i < ϕ4i . The effect of (7.56) is to keep the estimated parameters bounded
for all t ≥ t0 , which as discussed before allows at the very least to get ultimate
boundedness for s and r. To show this note that the ith element of ϕ̃˙ satisfies
T
ϕ̃˙ i = ϕ̂˙i = −γi β Ŷ fi + yTai so + fbi , (7.57)
where γi is the ith element of the diagonal of for i = 1, . . . , p. Define the positive
definite function
1 2
V = ϕ̃ , (7.58)
2γi i
whose derivative along (7.57) satisfies

T
V̇ ≤ |ϕ̃i | · |β Ŷ fi + yTai so | (7.59)
if ϕ2i ≤ ϕ̂i ≤ ϕ3i , but quite obviously ϕ̂i is bounded. On the contrary, if ϕ̂i < ϕ2i ≤
ϕmi then sign(ϕ̃i ) = −1, while if ϕMi ≤ ϕ3i < ϕ̂i then sign(ϕ̃i ) = +1, so that one has
ϕ2i − ϕ̂i T
V̇ ≤ − ρi − 1 |ϕ̃i | · |β Ŷ fi + yTai so |, (7.60)
ϕ2i − ϕ1i
for ϕ2i ≤ ϕ̂i ≤ ϕ3i and
ϕ̂i − ϕ3i T
V̇ ≤ − ρi − 1 |ϕ̃i | · |β Ŷ fi + yTai so |, (7.61)
ϕ4i − ϕ3i
for ϕMi ≤ ϕ3i < ϕ̂i . But, due to the restriction on the initial condition, the worst
possible case for (7.60) and (7.61) can be ϕ̂i = ϕ1i or ϕ̂i = ϕ4i , respectively, so that
they become
V̇ ≤ −(ρi − 1)|ϕ̃i | · |βwi + yTai so |. (7.62)
Since ρi > 1 then V̇ < 0 implying that |ϕ̃i | diminishes its value and thus it has to
hold ϕ1i ≤ ϕ̂i (t) ≤ ϕ4i . This shows the boundedness of the estimated parameters
gotten from (7.55) and as a direct consequence the possibility of achieving ultimate
boundedness with arbitrarily small bound for s and r.
Finally, for the unaware reader it may be difficult to understand at first sight
why filtering the regressor eliminates acceleration terms. The following example is
intended to clarify this issue.
Example
Consider the regressor presented in the example after Property 3.14:

⎡ ⎤
q̈1 c2 q̈1 + 13 + 21 c2 q̈2 − 21 s2 q̇2 q̇1 − 21 s2 q̇1 + 21 s2 q̇2 q̇2 c1 c1 + 21 c12
⎢ ⎥
⎢ ⎥ ϕ = τ,
⎣ ⎦
0 1 + 1 c q̈ + 1 q̈ + 1 s q̇ q̇ 0 1c
3 2 2 1 3 2 2 2 1 1 2 12

Y(q̈,q̇,q)
with s2 = sin(θ2 ), c1 = cos(θ1 ), c2 = cos(θ2 ), c12 = cos(θ1 + θ2 ) and the parameter

vector given by
⎡ ⎤ ⎡ ⎤
1 m l2 + 4 m l2
ϕ1
3 1 3 2
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎢
⎥
⎥
⎢ ϕ2 ⎥ ⎢ m2 l 2 ⎥
⎢ ⎥ ⎢ ⎥
ϕ=⎢
⎢ ⎥=⎢
⎥ ⎥.
⎢
⎢ ϕ3 ⎥ ⎢ ⎥
⎢ ⎥ ⎢
1m g l ⎥
⎣ ⎦ ⎣ 2 1 0 ⎥
⎦
ϕ4 m2 g0 l
If yij is the ijth element of Y(q, q̇, q̈), for i = 1, 2 and j = 1, . . . , 4, then according
to (7.41) every element yfij of Y f (q, q̇) can be computed by solving
ẏfij = −λf yfij + λf yij ,
whose solution is given by
t
−λf t
yfij (t) = yfij (0)e + λf e−λf (t−ϕ) yij (ϕ)dϕ
0
t
−λf t −λf t
= yfij (0)e + λf e eλf ϕ yij (ϕ)dϕ.
0
Without lost of generality it can be chosen yfij (0) = 0 to get
t
yfij (t) = λf e−λf t eλf ϕ yij (ϕ)dϕ.
0
In general, it is not possible to compute a closed form solution of the previous rela-
tionship and it has to be solved online, but to eliminate acceleration terms integration
by parts can be used according to

vdw = vw − wdv. (7.63)
For example, for the element y11 = q̈1 it has to be solved
t
−λf t
yf11 (t) = λf e eλf ϕ q̈1 dϕ.
0
By choosing
dw = q̈1 dϕ and v = e λf ϕ ,
to get
⎛ ⎞
t t
yf11 = λf e −λf t ⎝e q̇1 −
λf t
λf e λf ϕ
q̇1 dϕ ⎠ = λf q̇1 − λ2f e−λf (t−ϕ) q̇1 dϕ,
0 0
it can be appreciated that q̈1 has disappeared. But consider also the factor c2 q̈1 in
yf22 . For that case
t t
λf ϕ λf t
λf ϕ
e cos(q2 )q̈1 dϕ = e cos(q2 )q̇1 − λf e cos(q2 ) − eλf ϕ sin(q2 )q̇2 q̇1 dϕ.
0 0
Therefore the partial solution for c2 q̈1 is given by
t
λf cos(q2 )q̇1 − λf e−λf (t−ϕ) (λf cos(q2 )q̇1 − sin(q2 )q̇2 q̇1 ) dϕ,
0
where once again the acceleration has disappeared. The rest of yf22 and of the elements
of Y f (q, q̇) can be computed in the same way.
7.3 Robust Control
Adaptive control is a good option to compensate the lack of an accurate model param-
eter vector. However, an alternative consists in working with a constant parameter
vector and to add a robust term to the nominal control law aimed at compensating
the inaccuracies. Similar to Assumption 7.1 it is made:
Assumption 7.2 For a nominal constant value of the parameter vector ϕ, denoted
as ϕ 0 , an upper bound ρ < ∞ for the parameter error ϕ̃ is known, i.e.
ϕ̃ = ϕ 0 − ϕ ≤ ρ (7.64)
holds.
This way, instead of (7.1) one has a nominal control law given by
τ 0 = H 0 (q)q̈r + C 0 (q, q̇)q̇r + D0 q̇r + g0 (q) − K d s = Y a ϕ 0 − K d s, (7.65)
with K d > O as usual, Y a = Y(q, q̇, q̇r , q̈r ) as in (7.3), and (5.1), (5.49) and (5.50)
have been employed. Since in general ϕ 0 = ϕ, the control law (7.65) can be com-
plemented in the following fashion
τ = τ 0 + Y a f = Y a (ϕ 0 + f ) − K d s, (7.66)
where f ∈ Rp is designed to compensate the error ϕ̃ = ϕ 0 − ϕ defined similar

to (7.7). According to (7.8), it is not difficult to show that the closed loop dynamics
for s is given by
H(q)ṡ + C(q, q̇)s + K D s = Y a (ϕ̃ + f ), (7.67)
where K D is defined as before. The last equation shows that the term f can directly
be used to deal with the error ϕ̃. A possibility is to choose
⎧
⎪
⎪ Y Ta s
⎨ −ρ Y T s if Y a s > η
T
f = a
(7.68)
⎪
⎪
⎩ ρ
− η Y Ta s if Y Ta s ≤ η
Note that this term is continuous for η > 0. To show the effect of f consider once
again
1 T
V (t, s) = s H(q)s,
2
α1 (s) ≤ V (t, s) ≤ α2 (s),
with
1 1
α1 (s) = λh s2 and α2 (s) = λH s2 ,
2 2
where λh and λH are given in Property 3.7. The derivative of V (s) along (7.67) is
given by
V̇ (t) = −sT K D ṡ + sT Y a (ϕ̃ + f )

≤ −λmin (K d )s2 + sT Y a (ϕ̃ + f ). (7.69)
Assume now that Y Ta s > η. This means that
Y Ta s
f = −ρ ,
Y Ta s
and therefore it is
T Y Ts
sT Y a (ϕ̃ + f ) = Y Ta s ϕ̃ − ρ aT
Y a s
Y Ta s2
≤ Y Ta sϕ̃ − ρ
Y Ta s
≤ Y Ta sρ − ρY Ta s = 0.
7.3 Robust Control 181
On the contrary, if Y Ta s ≤ η, then
ρ
f = − Y Ta s,
η
and therefore it is
T ρ
sT Y a (ϕ̃ + f ) = Y Ta s ϕ̃ − Y Ta s
η
ρ T 2
≤ Y a sρ − Y a s
T
η
1
= ρ Y Ta s − Y Ta s2
η
1
= ρY Ta s 1 − Y Ta s , (7.70)
η
which is valid for 0 ≤ Y Ta s ≤ η and it is always positive except for the border
values where it becomes zero. The worst possible case is given at the maximum with
respect to Y Ta s, which can be computed according to
∂ 1 2 T
Y Ta s − Y Ta s2 =1− Y s.
∂Y a s
T η η a
Therefore, the maximum is at

η
Y Ta s = .
2
By substituting in (7.70) one has
η
sT Y a (ϕ̃ + f ) ≤ ρ . (7.71)
4
Of course, (7.71) can be considered valid even if η < Y Ta s, so that substituting
in (7.69) yields to
η
V̇ ≤ −λmin (K d )s2 + ρ
4
1 1 η
≤ − λmin (K d )s − λmin (K d )s2 + ρ .
2
2 2 4
This way it is easy to see that if
η 1
ρ ≤ λmin (K d )s2 ,
4 2
then V̇ < 0. This is equivalent to

ρη
≤ s2 ,
2λmin (K d )
or
$
ρη
μ= ≤ s.
2λmin (K d )
Therefore, if s ≥ μ it is
1
V̇ ≤ − λmin (K d )s2 = −W3 (s),
2
which complies with condition (4.53) of Theorem 4.5. Note that this time it was not
really necessary to define a region D because D ≡ Rn and the result holds for any
initial condition s(t0 ) or equivalently for any e(t0 ) and ė(t0 ). Finally, the ultimate
bound can be computed using (4.73), which for this case is
λH ρ ηλH
bf = μ= .
λH 2λmin (K d )λH
Clearly, bf can be made arbitrarily small by setting η also arbitrarily small. As a

direct consequence, both e and ė can be made arbitrarily small.
7.4 Control-Observer Robust Scheme
Just as made before for the adaptive scheme, in this section the tracking control
problem of rigid robot arms with model parameter uncertainty and without velocity
measurements is studied. Consider the usual definitions (5.1), (6.1), (6.43), (6.45)
and (6.73) i.e.
e = q − qd
z = q − q̂
q̇ = q̂˙ − z z
o
r = q̇ − q̇o = ż + z z
eo = q̂ − qd ,
ˆ denotes the esti-

with z ∈ Rn×n a diagonal positive definite matrix and where (·)
mated (or nominal) value of (·). It is introduced
7.4 Control-Observer Robust Scheme 183
q̇r = q̇d − eo = q̇d − e + z (7.72)

s = q̇ − q̇r = ė + e − z, (7.73)
with ∈ Rn×n a diagonal positive definite matrix. Let ϕ 0 be the nominal (constant)
value of the robot model parameters vector, given by ϕ, while for the parameter error
vector ϕ̃ = ϕ 0 − ϕ, the following assumption is made:
Assumption 7.3 Let ϕ̃i be the ith element of ϕ̃. For i = 1, . . . , p bounds are known
such that it holds
ϕ̃i ≤ ϕmi . (7.74)
As before, the bounds ϕmi can be set arbitrarily large if necessary. Similar to (6.39)
and (6.40), the following linear observer is proposed:
q̂˙ = ξ + z z + kd z (7.75)
ξ̇ = q̈r + kd z z, (7.76)
˙ The proposed
where kd is a positive real constant and q̂ is obtained by integrating q̂.
controller is given by

τ = H 0 (q)q̈r + C 0 (q, q̇r )q̇r + D0 q̇r + g0 (q) − K d q̇o − q̇r + f

= Y a ϕ 0 − K d q̇o − q̇r + f , (7.77)
where K d , ∈ Rn×n is a diagonal positive definite matrix and as before
Y a = Y(q, q̇r , q̈r ).
To design the robust term f , denote the columns of Y a as yai ∈ Rn , for i = 1, . . . , p.

In view of Assumption 7.3, the vector f can be defined as

%
p
ϕmi yTai ŝ + r̂
f =− yai , (7.78)
i=1
yTai ŝ + r̂ + ηi
where ŝ + r̂ = q̂˙ − q̇d + e, and ηi , i = 1, . . . , p, are positive constants. Since q̇o −
q̇r = s − r, (7.77) can be written as
τ = H(q)q̈r + C(q, q̇r )q̇r + Dq̇r + g(q) + Y a ϕ̃ − K d (s − r) + f . (7.79)

By substituting (7.79) in the robot model (3.44) and using Property 3.4 to have
C(q, q̇r )q̇r = C(q, q̇)q̇r + C(q, q̇r )q̇r − C(q, q̇)q̇r
= C(q, q̇)q̇r − C(q, q̇r )s, (7.80)
the tracking error closed loop dynamics satisfies
H(q)ṡ + C(q, q̇)s + K D s = K d r − C(q, q̇r )s + Y a ϕ̃ + f , (7.81)
where K D = D + K d . As to the observer error dynamics from (7.75) one has
q̂¨ = ξ̇ + z ż + kd ż, (7.82)
which substituted in (7.76) leads to
ṙ + kd r = ṡ. (7.83)
By multiplying this equation by H(q) and using (7.81), one gets
H(q)ṙ + kd H(q)r = −C(q, q̇)s − K D s + K d r − C(q, q̇r )s + Y a ϕ̃ + f . (7.84)
Using once again Property 3.4 to have
C(q, q̇)s + C(q, q̇r )s = C(q, q̇)r − C(q, q̇)r + C(q, q̇ + q̇r )s, (7.85)
yields to
H(q)ṙ = − C(q, q̇)r − H rd r − K D s

+ C(q, q̇)r − C(q, q̇ + q̇r )s + Y a θ̃ + f , (7.86)
where H rd = kd H(q) − K d . Define for (7.81) and (7.86) the state x as

s
x= , (7.87)
r
for which a region of the form (4.28) is defined as

D = x ∈ R2n \ x < ,
for some 0 < < ∞. As usual, the key idea is that whenever x ∈ D then the bound-
edness of r implies that of z and ż due to ż = −z z + r. On the other hand, the
boundedness of s and z implies from (7.73) that e and ė must be bounded. Also, since
qd and its derivatives are bounded by assumption then so are q and q̇. Therefore,
from (7.72) q̇r and q̈r must be bounded, and as direct consequence Y a (q, q̇r , q̈r ) is
bounded as well. This in turn means that f is bounded according to (7.78) and so
must be then ṡ in (7.81) in view of Property 3.1. From (7.86) this means that ṙ is
bounded as well. Consider
1 T 1
V (x) = s H(q)s + rT H(q)r, (7.88)
2 2
α1 (x) ≤ V (t, x) ≤ α2 (x),
with
1 1
α1 (x) = λh x2 and α2 (x) = λh x2 . (7.89)
2 2
λh and λH are given in Property 3.7. By taking into account Property 3.2, compute
the derivative of V (x) along (7.81) and (7.86) to get
V̇ = − sT K D s + sT K d r − sT C(q, q̇r )s + (s + r)T (Y a ϕ̃ + f )

− rT H rd r − rT K D s + rT C(q, q̇)r − rT C(q, q̇ + q̇r )s. (7.90)
For x ∈ D there must exist bounds such that
q̇ ≤ vm
q̇r ≤ vrm
hold. Furthermore, define
%
p
μa = 2 max yai ϕmi ,
i=1
so that
μa
Y a ϕ̃ ≤ ,
2
and

%p
ϕmi yai yTai ŝ + r̂ μa
f ≤ ≤ ,
i=1
yai ŝ + r̂ + ηi
T 2
hold. Then, by using K D = D + K d and taking into account that Properties 3.7, 3.9
and 3.10 one has
V̇ ≤ − λmin (K d )s2 + kc vrm s2 + μa (s + r)

− (kD λh − λmax (K d ))r2 + λmax (D)sr + kc vm r2
+ kc (vm + vrm )rs. (7.91)
Therefore, by choosing
λmin (K d ) ≥ 2δ + 1 + kc vrm (7.92)

2δ + λmax (K d ) + kc vm + 1
(λ (D)
4 max
+ kc (vm + vrm )) 2
kd ≥ , (7.93)
λH
one gets from (7.91)
V̇ ≤ −δx2 − x(δx − μa ). (7.94)
For
μa
x ≥ =μ (7.95)
δ
it is
V̇ ≤ −δx2 = −W3 (x), (7.96)
subject to
λH
μ< , (7.97)
λh
so that condition (4.53) of Theorem 4.5 is satisfied, and as consequence the trajec-
tories are ultimately bounded according to (4.73), i.e.
λH
x ≤ μ = bf , ∀ t ≥ t0 + T , (7.98)
λh
for some T > 0. At this point, it is evident that the smaller one wishes bf to be the
larger control gains must be. However, f tries to avoid using large gains by mimicking
up to some point (7.68). To understand this, for a small bf it holds ŝ + r̂ ≈ s + r and
therefore q̂˙ ≈ q̇. Make this substitution in (7.78) to have the term (s + r)T (Y a ϕ̃ + f )
in (7.90) approximated by
(s + r)T (Y a ϕ̃ + f )
%
p
ϕmi yTai (s + r) 2
≤ yTai (s + r) ϕmi −
i=1
yTai (s + r) + ηi
%p
≤ ϕmi ηi = η. (7.99)
i=1
In other words, if q̂˙ ≈ q̇, f becomes a robust term like the one given in (7.68),
where joint velocities are available. Using the same arguments as in Sect. 7.3, it is
straightforward to show that making the parameters ηi small enough may do x
tend to an even smaller region than the nominal one given by (7.98). While this is
not a formal proof, it helps to understand how f works.
7.5 Generalized Proportional Integral (GPI) Observer
In the previous sections it was taken advantage of Property 3.14 to compensate model
parameters inaccuracies. The main disadvantage of the approaches considered before
is that the model structure should be well known. This does not allow to deal explicitly
with non modeled dynamics. The Generalized Proportional Integral (GPI) observers
attempt to compensate non modeled dynamics with p-degrees polynomials and they
represent an alternative. To illustrate this technique, consider the computed torque
control law seen in Sect. 5.3 rewritten in the following form
& '
τ = H(q) q̈d − 2ζ ωn ė − ω2n e + C(q, q̇)q̇ + Dq̇ + g(q), (7.100)
where e = q − qd is the tracking error given in (5.1), and ζ and ωn ∈ Rn×n are positive
definite diagonal matrices. This leads to the closed loop dynamics
ë + 2ζ ωn ė + ω2n e = 0, (7.101)
which guarantees that tracking errors will tend to zero. Although theoretically the
control problem is exactly solved, cancelling C(q, q̇)q̇ + Dq̇ + g(q) is not really
possible. A basic GPI observer requires only the inertia matrix H(q) at the cost of
not achieving exact tracking but ultimate boundedness. To understand this approach,
assume that for any desired trajectory qd exists an ideal yet unknown control input
τ ∗ achieving exact tracking. For that case it must hold
H(qd )q̈d + C(qd , q̇d )q̇d + Dq̇d + g(qd ) = τ ∗ . (7.102)
Then one can form the closed loop tracking error dynamics as
ë = q̈ − q̈d = H −1 (q)τ + z, (7.103)

where z ∈ Rn is defined as
& '
z = − H −1 (q) C(q, q̇)q̇ + Dq̇ + g(q)
& '
+ H −1 (qd ) C(qd , q̇d )q̇d + Dq̇d + g(qd ) − H −1 (qd )τ ∗ . (7.104)
To recover (7.101) the ideal control law should be

& '
τ = H(q) −2ζ ωn ė − ω2n e − z . (7.105)
But (7.105) is not implementable because z is unavailable. The goal of the GPI design
is to estimate z and ė (not q̇!) simultaneously so that (7.105) becomes
( )
τ = H(q) −2ζ ωn ėˆ − ω2n e − ẑ , (7.106)
where (ˆ·) is the estimate of (·). The closed loop dynamics is then given by
ë + 2ζ ωn ė + ω2n e = 2ζ ωn ẽ2 + z̃, (7.107)
with the estimation errors defined as
ẽ2 = ė − ėˆ (7.108)

z̃ = z − ẑ. (7.109)
After (7.107), if both ẽ2 and z̃ can be made arbitrarily small, then e and ė can also
become arbitrarily small. The main problem is that z in (7.104) actually lumps all
unknown or uncertain terms and therefore it does not even own a well defined struc-
ture. To find a substitute for the dynamic behavior of z assume that it has a smooth
enough behavior so that in any time interval [t1 , t2 ] it can be approximated by a
polynomial of the form
%
p−1
z= ai t i + r, (7.110)
i=0
with each ai being a n-vector of constant coefficients for i = 0, . . . , p − 1. For smooth

enough it is meant that at least the first p derivatives of the residual term r exist (i.e.
up to r(p) ). This is illustrated in Fig. 7.1. The disadvantages of using (7.110) appear to
be evident. First of all the coefficients of the polynomials might change for two con-
secutive time intervals [t1 , t2 ] and [t2 , t3 ] and this for t ∈ [t0 , ∞). Second, the larger
the time interval the bigger the order of the polynomial. But the main disadvantage
is that z is unknown so that the polynomials cannot be computed anyway.
7.5 Generalized Proportional Integral (GPI) Observer 189
z(t) p−1

z(t) = ai ti + r()
i=0
t2 t
t1
Fig. 7.1 Approximation of z by a polynomial on the time interval [t1 , t2 ].
Now, consider the fact that the constant coefficients in (7.110) allow to get the
following equivalent description to describe z which is valid for t ∈ [t0 , ∞):
ż = ż1 = z2 (7.111)
..
.
żp−1 = zp = ż(p−1) (7.112)
(p) (p)
żp = r = ż . (7.113)
Recall that it has been assumed that r(p) exists. On the other hand, the polynomial
description in (7.110) is observable from the measurable position coordinate. Indeed,
from (7.103) it is
z = ë − H −1 (q)τ = q̈ − q̈d (t) − H −1 (q)τ , (7.114)
meaning that z can be written in terms of the output q, a finite number of its time
derivatives, the known quantity qd (t), and the control input, τ . Hence, z is observable.
Therefore, a linear Luenberger observer can be designed to estimate z, for which it
is necessary to rewrite model (7.103) as
ė = e2 (7.115)
−1
ė2 = H (q)τ + z1 , (7.116)
where it has been set z1 = z and e2 = ė. Based on (7.111)–(7.113) and (7.115)–
(7.116) and by defining
ėˆ = ê2 (7.117)

ẑ = ẑ1 (7.118)
ẽ = e − ê, (7.119)
the following linear observer can be introduced
ê˙ = ê2 + λp+1 ẽ (7.120)

ê˙ 2 = H −1 (q)τ + ẑ1 + λp ẽ (7.121)
ẑ˙ 1 = ẑ2 + λp−1 ẽ (7.122)
..
.
ẑ˙ p−1 = ẑp + λ1 ẽ (7.123)
ẑ˙ p = λ0 ẽ, (7.124)
where λ0 , . . . , λp+1 ∈ Rn×n are diagonal positive definite matrices. System (7.111)–
(7.113) and (7.115)–(7.116) in closed loop with the GPI control–observer scheme
given by (7.106) and (7.120)–(7.124) delivers the error dynamics
ẽ˙ = ẽ2 − λp+1 ẽ (7.125)

ẽ˙ 2 = z̃1 − λp ẽ (7.126)
z̃˙ 1 = z̃2 − λp−1 ẽ (7.127)
..
.
z̃˙ p−1 = z̃p − λ1 ẽ (7.128)
z̃˙ p = r(p) − λ0 ẽ. (7.129)
By combining (7.125)–(7.126) one gets
ẽ¨ + λp+1 ẽ˙ + λp ẽ = z̃1 , (7.130)
and from (7.127)–(7.129) it is

(p)
z̃1 = r(p) − λ0 ẽ − · · · − λp−1 ẽ(p−1) . (7.131)
By recalling that z1 = z, one gets after (7.130) and (7.131)
dp
ẽ(p+2) + λp+1 ẽ(p+1) + · · · + λ0 ẽ = r(p) = z. (7.132)
dt p
7.5 Generalized Proportional Integral (GPI) Observer 191
Rewrite (7.132) as
ẋ = Ax + Br(p) , (7.133)
where
⎡ ⎤
ẽ
⎢ .. ⎥
x=⎣ . ⎦ (7.134)
(p+1)
ẽ
is the error state and A ∈ Rn(p+1)×n(p+1) and B ∈ Rn(p+1)×n are constant matrices
given by
⎡ ⎤
O I ··· O
⎢ .. .. ⎥
⎢ . ⎥
A=⎢ . ⎥ (7.135)
⎣ O O ··· I ⎦
−λ0 −λ1 · · · −λp+1
& 'T
B = O ··· O I . (7.136)
Then, choose {λ0 , . . . , λp+1 } so that polynomials of degree p + 2
ρ(s) = sp+2 I + λp+1 sp+1 + · · · + λ1 s + λ0 (7.137)
be Hurwitz. Note that the scalar s to denote the Laplace variable should not be
confused with the vector sliding variable s. This renders system (7.133) stable. Define
the following region of the form (4.28), i.e.

D = x ∈ Rn(p+1) \ x < , (7.138)
where is a positive constant that can be arbitrarily large. By using a rather involved
procedure it can be shown that whenever x ∈ D, then any signal of interest is bounded
and the state x is ultimately bounded with a final bound that can be made arbitrarily
small by choosing the roots of (7.137) as far on left hand side of the s plane as
possible. The proof is skipped because it does not rely directly on Theorem 4.5.
7.6 GPI Observer Without Inertia Matrix
It is interesting to note that the observer (7.120)–(7.124) needs only the inertia matrix
to be known for implementation, but since in the end there is no reason at all to assume
it well known while the rest of the model is not, a possibility to omit it as well is
the following. When the robot model and velocity measurements are available, it has
been shown that the control law (5.51), i.e.
τ = H(q)q̈r + C(q, q̇)q̇r + Dq̇r + g(q) − K d s,
is a very good option and where (5.49) and (5.50), i.e.
q̇r = q̇d − e
s = q̇ − q̇r = ė + e,
have been used with K d , ∈ Rn×n diagonal positive definite matrices. In closed loop
one gets
H(q)ṡ + C(q, q̇)s + K D s = 0, (7.139)
where K D = D + K d . Instead of (7.103) one gets
ë = −ė + H −1 (q)(τ − C(q, q̇)s) + z, (7.140)
with
z = ė − q̈r − H −1 (q){C(q, q̇)q̇r + Dq̇ + g(q)}. (7.141)
However, the control law can no longer be given by (7.106). Instead, it can be used
τ = −K d (ê˙ + e) − H(q)ẑ = −K d s + K d ẽ˙ − H(q)ẑ, (7.142)
˙ Substituting (7.142) in (7.140) leads to

where ėˆ has been replaced by ê.
H(q)ṡ + C(q, q̇)s + K d s = K d ẽ˙ + H(q)z̃, (7.143)
so that if both ẽ˙ and z̃ can be made arbitrarily small, then it can easily be shown that s
(and as a direct consequence e and ė) can be made arbitrarily small as well. However,
the control law (7.142) still relies on the exact knowledge of the inertia matrix H(q).
Suppose that it is unknown and that it is approximated by the simple substitution
H(q) ≡ I. (7.144)
Then, the control law (7.142) becomes
τ = −K d (ê˙ + e) − ẑ = −K d s + K d ẽ˙ − ẑ. (7.145)
In closed loop with (7.140) one gets
H(q)ṡ + C(q, q̇)s + K d s = K d ẽ˙ + H(q)z − ẑ. (7.146)

7.6 GPI Observer Without Inertia Matrix 193
This last equation makes apparent that ẑ tries to compensate H(q)z rather than z
alone. For this reason it is more convenient to redefine z in (7.141) as

z = H(q) ė − q̈r − H −1 (q){C(q, q̇)q̇r + Dq̇ + g(q)}

= H(q) ė − q̈r − C(q, q̇)q̇r − Dq̇ − g(q). (7.147)
This makes the corresponding error dynamics be
ė = e2 (7.148)
−1 −1
ė2 = H (q)(τ − C(q, q̇)s) − ė + H (q)z1 (7.149)
ż1 = z2 (7.150)
..
.
żp−1 = zp (7.151)
(p)
żp = r , (7.152)
where z1 = z has been used as before and it is assumed that r(p) exists. The observer
is now given by
ê˙ = ê2 − ê + (λp+1 − )ẽ (7.153)

ê˙ 2 = λp ẽ (7.154)
ẑ˙ 1 = ẑ2 + λp−1 ẽ (7.155)
..
.
ẑ˙ p−1 = ẑp + λ1 ẽ (7.156)
ẑ˙ p = λ0 ẽ, (7.157)
where τ is not employed in (7.154) because it cannot be canceled exactly and thus
its introduction does not represent any advantage. Also, instead of (7.146) one has
H(q)ṡ + C(q, q̇)s + K d s = K d ẽ˙ + z̃. (7.158)
On the other hand, from (7.150)–(7.152) and (7.155)–(7.157) one can compute
z̃(p) = r(p) − λ0 ẽ − · · · − λp−1 ẽ(p−1) , (7.159)
while from (7.148)–(7.149) and (7.153)–(7.154) one gets
ẽ˙ = ẽ2 + ê − (λp+1 − )ẽ (7.160)

ẽ˙ 2 = H (q)(τ − C(q, q̇)s) − ė + H (q)z1 − λp ẽ,
−1 −1
(7.161)
or
ẽ¨ − ê˙ + (λp+1 − )ẽ˙ =H −1 (q)(τ − C(q, q̇)s)

− ė + H −1 (q)z1 − λp ẽ. (7.162)
This means that
ẽ¨ + λp+1 ẽ˙ + λp ẽ = H −1 (q)(τ − C(q, q̇)s) + H −1 (q)z1 . (7.163)
By substituting τ from (7.145) one has
ẽ¨ + λp+1 ẽ˙ + λp ẽ = − H −1 (q)(K d s + C(q, q̇)s)

˙
+ H −1 (q)z̃ + H −1 (q)K d ẽ, (7.164)
which can be rewritten as
ẽ¨ + λp+1 ẽ˙ + λp ẽ = H −1 (q)(−K d s + K d ẽ˙ − C(q, q̇)s) + H̄(q)z̃ + z̃, (7.165)
with H̄ = H −1 (q) − I. By defining
f = H −1 (q)(−K d s + K d ẽ˙ − C(q, q̇)s) + H̄(q)z̃, (7.166)
and by computing the pth derivative of (7.165) and substituting in (7.159) one gets
ẽ(p+2) + λp+1 ẽ(p+1) + · · · + λ0 ẽ = r(p) + f (p) (t). (7.167)
This means that instead of using (7.133) one has
ẋ = Ax + Br̄f , (7.168)
with r̄f = r(p) + f (p) . As before, if a region of interest of the form (4.28) is defined,
then it can be shown that a combination of roots for the polynomials in (7.137) can
always be chosen to make the sliding variables (s, ṡ) and the tracking errors (e, ė, ë)
in (7.158) and the observation errors x in (7.134) arbitrarily small. Furthermore, the
estimate ẑ1 becomes arbitrarily close to the real value z. Once again, the proof is
omitted because it is very involved and not based on Theorem 4.5.
In this section, an experimental evaluation of the control schemes described in the

sections above is presented. For such evaluation, the Phantom Touch robot described
in Chap. 14 is employed. The 3 degrees-of-freedom configuration characterized in
Sect. 14.2 is utilized, i.e. the last three non-actuated joints are mechanically blocked.
The desired trajectory in joint coordinates for all the experiments below is given
by ⎡ ⎤
−30 ◦ + 30 ◦ (1 − cos(π t))
qd = ⎣ 20 ◦ + 30 ◦ (1 − cos(π t)) ⎦ , (7.169)
−60 ◦ − 30 ◦ (1 − cos(π t))
whereas the desired velocities and accelerations, q̇d and q̈d , are obtained by taking
the time derivative of the desired joint position.
Slotine–Li Adaptive Controller
Tuning Guide
The necessary equations to implement the Slotine–Li adaptive controller are (5.1),
(5.49), (5.50), (7.4), and (7.13). To tune the controller consider the following sug-
gestions.
• Set an initial guess for the parameters vector, ϕ̂(t0 ), it can be the zero vector if no
knowledge of the parameters is available.
• At first, neglect the adaptive term, Y a ϕ̂, by setting = O, and tune the remaining
one as a standard PD controller by setting K d equal to the derivative gain matrix,
and set = K −1d K p , where K p is the proportional gain matrix.
• Now, set as a diagonal matrix, for which every element on the diagonal cor-
responds to the adaptation gain of a particular parameter. Start by setting very
small adaptation gains and gradually increase them until the parameters tend to a
constant value within a desired settling time.
Experiment
The gains chosen for the implementation of the Slotine–Li algorithm by following
the tuning guide are displayed in Table 7.1. The position tracking in joint coordi-
nates is shown in Fig. 7.2, and the corresponding tracking error in Fig. 7.3. The
joint velocities estimated by means of a derivative filter are shown along with the
desired joint velocities in Fig. 7.4. The time evolution of the estimated parame-
ters is shown in Fig. 7.5. The initial guess for the parameters vector was set to
& 'T
ϕ̂ = 0 0 0 0.02 0.02 0.02 0 0 . The first three parameters correspond to the iner-
tia matrix and Coriolis vector, the next three correspond to viscous friction and the
last two to the gravity vector.
Table 7.1 Chosen gains for Gain Value

the Slotine–Li controller
Kd diag{0.01, 0.01, 0.01}
diag{50, 50, 25}
1 × 10−5 diag{1, 1, 1, 1, 1, 1, 10, 10}
Fig. 7.2 Slotine–Li controller, joint position tracking: desired (· · · ), real (—)
Adaptive Scheme with Velocity Observers

Tuning Guide
The equations needed to implement the adaptive scheme of Sect. 7.2 are the tracking
variables (5.1), (5.49), (5.50), the observer Eqs. (6.1), (6.39)–(6.40), and (6.42)–
(6.44), the filter (7.40), the prediction error (7.44), the adaptation law (7.47), and the
control law (7.4). Note that the analytic filtered version of the regressor, obtained
through (7.42), and computed as in the example at the end of Sect. 7.2, is also needed.
• Set an initial guess for the parameters vector, ϕ̂(t0 ), it can be the zero vector if no
knowledge of the parameters is available.
• As in the case of the Slotine–Li controller, first neglect the adaptive term by setting
= O, and tune the remaining one as a standard PD controller by setting K d equal
to the derivative gain matrix, and = K −1d K p , where K p is the proportional gain
matrix.
• Set the observer gains kd and z satisfying (6.65)–(6.67) and (6.69)–(6.70), to
minimize the observation error z.
• Now, set β = 0, and proceed to tune the adaptation gains matrix as in the case
of the Slotine–Li algorithm.
• Finally, increase β gradually to improve the performance of the position tracking.
Fig. 7.3 Slotine–Li controller, joint position tracking error
Table 7.2 Chosen gains for the adaptive scheme of Sect. 7.2.
Gain Value
Kd diag{0.01, 0.01, 0.01}
diag{50, 50, 25}
1 × 10−5 diag{1, 1, 1, 1, 1, 1, 10, 10}
kd 200
z diag{1, 1, 1}
β 10
Experiment
The gains chosen for the implementation of the scheme of Sect. 7.2 by following
the tuning guide above are displayed in Table 7.2. The position tracking is dis-
played in Fig. 7.6, whereas the corresponding tracking error is shown in Fig. 7.7.
The joint velocities estimated by the proposed observer are shown along with the
desired joint velocities in Fig. 7.8. The time evolution of the estimated parame-
ters is shown in Fig. 7.9. The initial guess for the parameters vector was set to
& 'T
ϕ̂ = 0 0 0 0.02 0.02 0.02 0 0 as in the Slotine–Li scheme.
Fig. 7.4 Slotine–Li controller, joint velocities: desired (· · · ), numerical derivative (—)
Robust Controller
Tuning Guide
The equations required to implement the robust controller of Sect. 7.3 are the track-
ing variables (5.1), (5.49), (5.50), the control law (7.66), and the function defined
in (7.68). For this scheme, a nominal parameters vector ϕ 0 is also required. The
following directions might be useful when implementing this controller.
• At first, set ρ = 0 in (7.68), and tune the remaining one as a standard PD plus
Feedforward controller.
• Set ρ to satisfy (7.64).
• Finally, set η > 0, perhaps after looking at the time-plot of Y Ta s to obtain the desired
performance.
Fig. 7.5 Slotine–Li controller, estimated parameters: θ̂1 (—), θ̂2 (—), θ̂3 (—), θ̂4 (—), θ̂5 (– –),
θ̂6 (– –), θ̂7 (– –), θ̂8 (– –)
Table 7.3 Chosen gains for the robust controller of Sect. 7.3
Gain Value
Kd diag{0.01, 0.01, 0.01}
diag{50, 50, 25}
η 40
ρ 0.0005
Experiment
By following the above tuning guide, the gains chosen for the implementation of
the robust controller described in Sect. 7.3 are displayed in Table 7.3. The nominal
value for the parameters vector was set to
& 'T
ϕ 0 = 0.0045 0.0003 0.0045 0.0014 0.0012 0.0006 0.0049 0.013 ,
after the estimation reported in Sect. 14.2.5. The position tracking is displayed in
Fig. 7.10, whereas the corresponding tracking error is shown in Fig. 7.11. The joint
velocities obtained by a derivative filter are shown along with the desired joint veloc-
ities in Fig. 7.12.
Fig. 7.6 Adaptive scheme with velocity observers, joint position tracking: desired (· · · ), real (—)
Control-Observer Robust Scheme

Tuning Guide
The equations required to implement the control-observer robust scheme of Sect. 7.4
are the tracking variables (5.1), (7.72), the observer equations (6.42)–(6.44), and (6.1),
(7.75)–(7.76), the control law (7.77), and the function defined in (7.78). The tuning
procedure can be carried out as follows.
• As in the previous controller, first neglect the robustification term given by the
function f defined in (7.78), and tune the remaining ones as a standard PD plus
Feedforward controller (Kelly et al., 2005).
• Set the observer gains kd and z satisfying (6.65)–(6.67) and (6.69)–(6.70), to
• Set η very small, just to avoid the indeterminacy of f in (7.78).
• Set each ϕmi to satisfy (7.74).
Fig. 7.7 Adaptive scheme with velocity observers, joint position tracking error
Experiment
The gains chosen for the implementation of the scheme of Sect. 7.4 by following the
tuning guide above are displayed in Table 7.4. The nominal value for the parameters
vector was set to
& 'T
ϕ 0 = 0.0045 0.0003 0.0045 0.0014 0.0012 0.0006 0.0049 0.013 ,
after the estimation reported in Sect. 14.2.5. The position tracking is displayed in
Fig. 7.13, and the corresponding tracking error is shown in Fig. 7.14. The joint veloc-
ities estimated by the proposed observer are shown along with the desired joint
velocities in Fig. 7.15.
GPI Observer
Tuning Guide
To implement the GPI observer controller of Sect. 7.5, the required equations are the
tracking error (5.1), the observer Eqs. (7.117)–(7.124), and the control law (7.106).
The tuning procedure can be carried out as follows.
Fig. 7.8 Adaptive scheme with velocity observers, joint velocities: desired (· · · ), observer (—)
Table 7.4 Chosen gains for the control-observer scheme of Sect. 7.4
Gain Value
Kd diag{0.01, 0.01, 0.01}
diag{50, 50, 25}
kd 200
z diag{1, 1, 1}
η 1 × 10−6
ϕmi 5 × 10−5 , i = 1, 2, . . . , 8
• Select the degree of the polynomial p in (7.110) to define the order of the dynamic
extension that represents the internal model for the unknown dynamics. As a
rule, the higher p the accurate the signals the controller can reject, at the cost of
increasing undesired effects like the well known peaking phenomena arising when
employing high-gain observers. For the common tasks of robotic manipulation,
usually p = 2 is a good choice.
• Set the pole observers of the diagonal matrix polynomial (7.138) as far in the left
of the complex plane as the bandwidth allows it, and compute the observer gains
Fig. 7.9 Adaptive scheme with velocity observers, estimated parameters: θ̂1 (—), θ̂2 (—), θ̂3 (—),
θ̂4 (—), θ̂5 (– –), θ̂6 (– –), θ̂7 (– –), θ̂8 (– –)
Table 7.5 Chosen gains for the GPI observer scheme of Sect. 7.5
Gain Value
p 2
λ0 8.1 × 105 I
λ1 10.8 × 104 I
λ2 5.4 × 103 I
λ3 120I
ζ I
ωn 40I
0.4 s
λ0 , . . . , λp+1 . As a rule, the higher the sampling frequency, the larger can be these
poles in magnitude.
• Choose the constant matrices ζ and ωn according to a desired second-order system
response.
Fig. 7.10 Robust controller, joint position tracking: desired (· · · ), real (—)
Remark 7.1 To reduce the undesired peaking phenomena, it is convenient to imple-

ment a suitable clutch, which can be of the form:

sin8 ( πt ) if t ≤
fclutch = 2
1 if t >
where is the time duration of the peaking. Finally, multiply ê2 and ẑi , i = 1, . . . , p,
by fclutch .
Experiment
The gains chosen for the implementation of the scheme of Sect. 7.5 by following the
tuning guide above are displayed in Table 7.5. The position tracking is displayed in
Fig. 7.16, whereas the corresponding tracking error is shown in Fig. 7.17. The joint
velocities estimated by the proposed observer are shown along with the desired joint
velocities in Fig. 7.18.
Fig. 7.11 Robust controller, joint position tracking error
GPI Observer Without Inertia Matrix

Tuning Guide
The necessary equations to implement the GPI observer without inertia matrix
scheme of Sect. 7.6, are the tracking error (5.1), the observer equations (7.117)–
(7.119) and (7.153)–(7.157), and the control law (7.145). The following directives
might be useful when tuning the controller.
• Select the degree of the polynomial p in (7.110) to define the order of the dynamic
extension that represents the internal model for the unknown dynamics. As a rule,
the higher p the more accurately the signals the controller can reject, at the cost of
increasing undesired effects like the well known peaking phenomena arising when
employing high-gain observers. For the common tasks of robotic manipulation,
usually p = 2 is a good choice.
• Set the pole observers of the diagonal matrix polynomial (7.138) as far in the left
of the complex plane as the bandwidth allows it, and compute the observer gains
λ0 , . . . , λp+1 . As a rule, the higher the sampling frequency, the larger can be these
poles in magnitude.
Fig. 7.12 Robust controller, joint velocities: desired (· · · ), numerical derivative (—)
• At first, neglect the ẑ term in the control law (7.145), and tune the remaining one
as a standard PD controller by setting K d equal to the derivative gain matrix, and
= K −1d K p , where K p is the proportional gain matrix.
Remark 7.2 To improve the performance of the controller, another constant matrix
different from the identity in (7.144) can be used, which better approximates H(q),
e.g. H(q0 ), where q0 is any point on the desired joint trajectory.
Experiment
The gains chosen for the implementation of the scheme of Sect. 7.6 by following
the tuning guide above are displayed in Table 7.6. To improve the performance, the
constant matrix ⎡ ⎤
0.0049 0 0
H 0 = H(0) = ⎣ 0 0.0097 0.0048⎦ ,
0 0.0048 0.0045
obtained with the parameters estimated in Sect. 14.2.5, was employed instead the
identity in (7.144). The position tracking is displayed in Fig. 7.19, whereas the cor-
responding tracking error is shown in Fig. 7.20. The joint velocities estimated by the
proposed observer are shown along with the desired joint velocities in Fig. 7.21.
Fig. 7.13 Control-observer robust scheme, joint position tracking: desired (· · · ), real (—)
Table 7.6 Chosen gains for the GPI observer without inertia matrix scheme of Sect. 7.6
Gain Value
p 2
λ0 6.25 × 106 I
λ1 5 × 105 I
λ2 1.5 × 104 I
λ3 200I
Kd diag{0.05, 0.05, 0.05}
diag{20, 40, 20}
7.7.1 Performance Comparison
Since the desired trajectories are the same for all the above experiments, it is worth
to compare the performance of the controllers. Notice however, that two of the con-
trollers require velocity measurements, which are actually obtained through posi-
tions by means of filter derivatives. For this reason, only the performance of the
position tracking error is taken into account, by employing the Root-Square-Mean
Fig. 7.14 Control-observer robust scheme, joint position tracking error
Table 7.7 Comparison of the controllers’ position tracking performance

Scheme Joint 1 RMSE Joint 2 RMSE Joint 3 RMSE Sum
Slotine-Li 0.7352 1.1019 1.4429 3.28
Adaptive + Observers 0.9993 1.2612 1.5033 3.7638
Robust 0.5601 0.4679 0.3938 1.4218
Control-Observer robust 0.7242 0.6082 0.5117 1.8441
GPI Observer 0.5164 0.8808 1.0819 2.4791
GPIO without inertia matrix 0.3081 0.4652 0.3750 1.1483
Error (RMSE) index. The results of this comparison for each joint, along with the
sum of all joints RMSE, are displayed in Table 7.7.
It is worth to point out that the RMSE of the GPI observer described in Sect. 7.5 is
greatly affected by the peaking at the start of the trajectories. In fact, if the first 1 s of
the experiment is not taken into account to compute the RMSE, the GPI observer gets
second best performance, just below the GPIO without inertia matrix of Sect. 7.6.
Fig. 7.15 Control-observer robust scheme, joint velocities: desired (· · · ), observer (—)
Fig. 7.16 GPI Observer, joint position tracking: desired (· · · ), real (—)
Fig. 7.17 GPI Observer, joint position tracking error
Fig. 7.18 GPI Observer, joint velocities: desired (· · · ), observer (—)

Fig. 7.19 GPI Observer without inertia matrix, joint position tracking: desired (· · · ), real (—)
Fig. 7.20 GPI Observer without inertia matrix, joint position tracking error
Fig. 7.21 GPI Observer without inertia matrix, joint velocities: desired (· · · ), observer (—)
References
Anderson BDO (1977) Exponential stability of linear equations arising in adaptive identification.
IEEE Trans Autom Control 22(1):83–88
Anderson BDO, Moore JB (1969) New results in linear system stability. SIAM J Control 7(3):398–
414
Arteaga-Pérez MA, Gutiérrez-Giles A (2014) On the GPI approach with unknown inertia matrix in
robot manipulators. Int J Control 87(4):844–860
Arteaga-Pérez MA, Kelly R (2004) Robot control without velocity measurements: new theory and
experimental results. IEEE Trans Robotics Autom 20(2):297–308
Arteaga-Pérez MA, Ortiz-Espinoza A, Romero JG, Espinoza-Pérez G (2020) On the adaptive control
of robot manipulators with velocity observers. Int J Robust Nonlinear Control 30:4371–4396
Diop S, Fliess M (1991) Nonlinear observability, identifiability and persistent trajectories. In: Pro-
ceedings of the 36th IEEE conference on decision and control, Brighton, UK, Dec 1991
Fliess M, Marquez R, Delaleau E, Sira-Ramírez H (2002) Correcteurs proportionnels intègraux
généralisés. ESAIM: Control Optim Calc Var 7(2):23–41
Ioannou PA, Sun J (1996) Robust adaptive control. Prentice-Hall, USA
Kalman RE (1960) Contributions to the theory of optimal control. Boletín de la Sociedad Matemática
Mexicana 5:102–119
Kelly R, Santibáñez V, Loria A (2005) Control of robot manipulators in joint space. Springer,
London
Khalil HK (2002) Nonlinear systems, 3rd edn. Prentice-Hall, Upper Saddle River, NJ
Morgan AP, Narendra KS (1977) On the uniform asymptotic stability of certain linear nonau-
tonomous differential equations. SIAM J Control Optim 15(1):5–24
References 213
Slotine JJE, Li W (1987) On the adaptive control of robot manipulators. Int J Robotics Res 6(3):49–
59
Slotine JJE, Li W (1989) Composite adaptive control of robot manipulators. Automatica 25(4):509–
519
Slotine JJE, Li W (1991) Applied nonlinear control. Prentice-Hall, Englewood Cliffs, NJ
Spong MW (1992) On the robust control of robot manipulators. IEEE Trans Autom Control
37(11):1782–1786
Tang Y, Arteaga-Pérez MA (1994) Adaptive control of robot manipulators based on passivity. IEEE
Trans Autom Control 39(9):1871–1875
Chapter 8
Force Control
Abstract When a robot gets in contact with its environment, represented by a rigid or
flexible surface for example, then the manipulators’ movements become constrained.
For that case it is advisable to implement directly or indirectly a force control strategy
to avoid damages both on the robot and to the environment. If the contact surface
is well known then it is also possible to change the control objective not only to
get position tracking but also force tracking or regulation. Some of the new arising
challenges are the possible lack of both velocities and force measurements, a poor
model of the environment or high friction effects. This chapter is meant to deal
with the force tracking control when the contact surface is assumed to be rigid and
frictionless so that the force and velocity components of the end-effector can be split
into two tangent planes.
8.1 Robot Force Control Without Dynamic Model
Assume that the end-effector of a robot manipulator gets in touch with its environment
represented in work space coordinates as in (3.84), i.e.
ϕ(x) = 0,
where the vector x ∈ Rn can be defined as in (2.172), i.e.

0
pn
x= ,
0
φn
where 0 φ n ∈ Rν is a parametrization of the orientation and 0 pn ∈ R(n−ν) is the end-

effector position,1 or it can be defined according to (2.183), i.e.
1The notation 0 pn is preferred for this chapter instead of 0 d n for being more congruent with the
environment description.
https://doi.org/10.1007/978-3-030-85980-0_8
216 8 Force Control
0
ṗn
ẋ = = J(q)q̇,
0
ωn
where J(q) ∈ R6×n is the geometric Jacobian of the manipulator and 0 ωn ∈ Rν is

the angular velocity of the end-effector. For simplicity’s sake assume that n = 6
and ν = 3 and that end-effector rotations are not constrained, so that any of the two
definitions of x can be employed to express (3.84). In fact, it can be rewritten as
ϕ(0 pn ) = ϕ(x) = 0, (8.1)
to make explicit that ϕ(x) = 0 depends indeed only on the upper part of x. This turns
out to be relevant if (2.183) is preferred because for that case x is given by
⎡ ⎤
0 0
pn
pn
x= = ⎣ t ⎦. (8.2)
0
φn 0
ωn dϑ
t0
As explained in Sect. 2.4.2, 0 φ n in (8.2) does not have any physical meaning. Still,
there is no analytical reason at all not to employ it as long as only force and not
torque control of the end-effector is pursued, i.e. only unconstrained orientation is
considered. On the other hand, since 0 φ n does not have any physical meaning it cannot
be measured, but for implementation online it can be approximated by employing
0
φ n (kT ) = 0 φ n ((k − 1)T ) + J ω (q){q(kT ) − q((k − 1)T )} (8.3)
where J ω (q) ∈ R3×n is the lower part of the geometric Jacobian J(q) and T is the
sampling time. One can further set2
⎤ ⎡
x
⎢ y ⎥
⎢ ⎥
⎢ z ⎥
x=⎢ ⎥
⎢ φx ⎥ . (8.4)
⎢ ⎥
⎣ φy ⎦
φz
Consider now the robot model of a manipulator in contact with its environment given
by (3.87), i.e.
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ − J T (q)J Tϕx λ,
2 Using x as vector and x as scalar (to designate the x-coordinate) should not cause any confusion.
8.1 Robot Force Control Without Dynamic Model 217
where (3.85) is used, i.e.
∂ϕ(x)
J ϕx = .
∂x
The goal of this section is to avoid using inverse kinematics by proposing a desired
position trajectory xd ∈ Rn and then defining a tracking error
x = x − xd (8.5)
directly in work space coordinates. It is assumed that xd and its first two derivatives
are bounded. Suppose that velocity measurements are not available. Then, an estimate
of x is given by x̂, and the observation error is defined in the same fashion as for joint
coordinates in (6.1), i.e.
z = x − x̂. (8.6)
Also, as done in (6.73), define the auxiliary error variable
eo = x̂ − xd = x − z. (8.7)
By considering a normalization of J Tϕx the Lagrange multipliers represent physically

the force applied to the surface, so that the force error can be defined as
λ = λ − λd , (8.8)
where λd ∈ Rm is the desired bounded force, with at least its first derivative bounded.
Consider Property 3.16 to define the sliding variable
s = Qx (ėo + x eo ) + J +
ϕx ξ 2 F = sp + sf , (8.9)
where x ∈ Rn×n and ξ 2 ∈ Rm×m are diagonal positive definite matrices and
F = λdϑ. (8.10)
t0
As made before, the design approach is based on making s tend to zero. Note that sp
and sf are orthogonal vectors, and that P x J + +
ϕx = J ϕx . Thus, they both become zero if
s → 0. But recall that J ϕx is assumed to be full rank and so must be J +
T
ϕx , so that
F = 0 (8.11)
218 8 Force Control
Fig. 8.1 Planes tangent to the surface at actual and desired positions. a Large tracking error; b small
tracking error
holds because ξ 2 is a positive definite matrix. However, the fact that sp becomes zero
does not necessarily implies that ėo and x eo will do it as well. To achieve this goal,
once again it will be taken advantage of a local stability analysis. For that instance,
rewrite first the tracking error as
x = x − xd = Qx (x)x + P x (x)x − Qx (xd )xd − P x (xd )xd . (8.12)
In Fig. 8.1 it can be appreciated that the smaller the tracking error, the more it
tends to belong to the plane tangent to the contact surface, which can be expressed
as
x ≈ Qx (x)(x − xd ) = Qx (x)x. (8.13)
How small the error must be for (8.13) to hold depends on how smooth the surface
is. After Property 3.16, if (8.13) is valid then one also has
ẋ ≈ Qx (x)(ẋ − ẋd ) = Qx (x)ẋ. (8.14)
Assumption 8.1 The approximations (8.13) and (8.14) are valid if
x ≤ ψ (8.15)
holds for some ψ > 0.
The previous assumption may be appear to be restrictive and somehow it is indeed

so. Basically, it requires a zero initial error and then an accurate tracking to keep (8.15)
holding. However, the main disadvantage is that the usual region of interest defined
to carry out a local stability analysis cannot be made arbitrarily large.
A case of particular interest is that of a rigid plane as contact surface. In fact, any
vector x can be split in the two orthogonal spaces described by Qx and P x given in
Property 3.16, i.e.
x = Qx x + P x x, (8.16)
where in general P x x = 0. This case is depicted in Fig. 8.2, where it can be appreciated
that the vector x lying in the space spanned by P x must be constant. This is easy to
understand because P x x represents the minimal distance to the surface. Note also
that for the plane J ϕx in (3.85) is constant and therefore so are Qx and P x . This in
turn from (8.16) implies that
ẋ = Qx ẋ, (8.17)
which fulfills (3.120) as well. As to xd , it must comply with (8.16), so that it is chosen
xd = Qx xd + P x xd ≡ Qx xd + P x x (8.18)
ẋd = Qx ẋd . (8.19)
Fig. 8.2 Rigid plane as

contact surface
220 8 Force Control
Assume that ẋ is not available and thus it has to be estimated. But the estimated x̂
has to comply with (8.16) as well, otherwise said, the estimated variable must have
form similar to (8.18)–(8.19), i.e.
x̂ = Qx x̂ + P x x (8.20)
x̂˙ = Q x̂,
˙
x (8.21)
where P x x is constant and known. To make sure that this goal is achieved, it can be
used
t
x̂ = Qx ˙
x̂dϑ + Px x (8.22)
t0
⎛ t
⎞
x̂˙ = Qx ⎝ẋd − x eo + kd z z(ϑ)dϑ + z z + kd z⎠ (8.23)

t0
to enforce x̂˙ to lie in the space spanned by Qx , while z ∈ Rn×n is a diagonal positive
definite matrix and kd is a positive constant. Obviously, one has P x x̂˙ = 0 and since
P x x = P x x̂ = P x xd , it holds
z = Qx z eo = Qx eo . (8.24)
In other words P x z = P x eo = 0. Since Qx ẋd = ẋd , then
ż = Qx ż ėo = Qx ėo . (8.25)
To take advantage of the previous relationships assume that Qx x = x Qx (for

example by setting x = kx I, with kx > 0). Then sp in (8.9) can be rewritten as
sp = Qx (ėo + x eo ) = ėo + x eo , (8.26)
from which it can be concluded that both eo and ėo tend to zero if sp tends to zero,
and for that it is enough that s does. Should that be the case, this can be used to show
that x, ẋ, z and ż do tend to zero. The design approach is based on the controller
described in Sect. 6.3, for which consider
ẋr = Qx (ẋd − x eo ) − J +
ϕx ξ 2 F − K γ σ , (8.27)
where K γ ∈ Rn×n is a diagonal positive definite matrix and σ ∈ Rn , with
t

σ = K β s(ϑ) + sign(s(ϑ)) dϑ (8.28)
t0
where σ (0) = 0 and K β ∈ Rn×n is a diagonal positive definite matrix, and
sign(s) = [sign(s11 ) · · · sign(s1n )]T , (8.29)
with s1i element of s, i = 1, . . . , n. Equivalently it can be written
d
σ = K β s + sign(s). (8.30)
dt
Then, consider
sx = ẋ − ẋr = Qx (ẋ + x eo ) + J +
ϕx ξ 2 F + K γ σ . (8.31)
The previous definitions allow to design the control-observer scheme directly in work
space coordinates, but instead of rewriting model (3.87) in those coordinates, it is
more convenient to define
sr = q̇ − q̇r = J −1 (q) (ẋ − ẋr ) = J −1 (q)sx , (8.32)
and to rewrite (3.87) in terms of sr as follows
H(q)ṡr + C(q, q̇)sr + Dsr = τ − J T (q)J Tϕx λ − ya , (8.33)
where
ya = H(q)q̈r + C(q, q̇)q̇r + Dq̇r + g(q). (8.34)
Prior to continue, it is made the following:
Assumption 8.2 The robot manipulator does not reach any singularity, so that
J −1 (q) always exits and it holds
q̇ = J −1 (q)ẋ. (8.35)
This assumption is not only necessary for (8.32) to be implementable, but it is

actually mandatory to guarantee that the end-effector can apply forces in the desired
directions. Now one can define the auxiliary variables
222 8 Force Control
ẋo = x̂˙ − z z (8.36)

r = ẋ − ẋo = ż + z z (8.37)
so = ẋo − ẋr . (8.38)
Based on all previous definitions, the proposed control law is
τ = −K p J −1 (q)so + J T (q)J Tϕx λd − J T (q)J Tϕx ξ 1 F, (8.39)
with K p ∈ Rn×n and ξ 1 ∈ Rm×m positive definite diagonal matrices. By substitut-

ing (8.39) in (8.33) and by taking into account that so = sx − r, one gets
H(q)ṡr + C(q, q̇)sr + K DP sr = −J T J Tϕx λ + K p J −1 r − J T J Tϕx ξ 1 F − ya ,

(8.40)
where K DP = D + K p . Equation (8.40) is related to the boundedness of the tracking

and force errors, but to describe the dynamics of the observation error, first of all
assume that z Qx = Qx z (once again, this can be done by choosing z = kz I, with
kz > 0). Then rewrite (8.23) as
t
x̂˙ − z Qx z − kd Qx z = Qx (ẋd − x eo ) + kd z Qx z(ϑ)dϑ. (8.41)

0
From (8.24) and (8.27), it is

t
ẋ − ẋr = ẋ − x̂˙ + z z + kd z + kd z z(ϑ)dϑ + J +

ϕx ξ 2 F + K γ σ . (8.42)
0
By taking into account (8.31), (8.36) and (8.37), it is
sx = r + kd r(ϑ)dϑ + J +
ϕx ξ 2 F + K γ σ . (8.43)
0
But, from (8.31) one has

t
r + kd r(ϑ)dϑ = Qx (ẋ + x eo ) . (8.44)

0
Alternatively, one can also write
ṙ + kd r = Qx (ẍ + x ėo ) . (8.45)

Now consider the following definition for the state of the error dynamics (8.10),
(8.40) and (8.45)
⎡ ⎤
sr
y = ⎣ r ⎦, (8.46)
F
for which a region of the form (4.28), i.e.

D = y ∈ R2n+m \ y < ,
is defined for some 0 < < ∞. On the contrary to the previous chapters, cannot be
made arbitrary large because it must be chosen small enough to comply with (8.15).
Certainly, for a plane ψ can be fairly large, but still is not really free. A stability
proof can be carried out following three basic steps: (a) The boundedness of y implies
the boundedness of any other closed loop variable. (b) With a proper choice of gains,
the observer (8.22)–(8.23) and the control law (8.39) guarantee the boundedness
of the state y in closed loop. (c) If all closed loop variables are bounded, then the
inclusion of the sign function in (8.28) make all errors tend to zero.
(a) It has to be shown that if y is bounded by , then any other signal is bounded.
This is completely similar to the procedure given in Sect. 6.3 and it proceeds as
follows. From (8.27) one has
− Qx (ẋd − x eo ) + J +
ϕx ξ 2 F = −ẋr − K γ σ . (8.47)
Then, from (8.9), (8.25) and (8.31) one gets after some manipulation
u = s + Kγ σ , (8.48)
with
u = sx − ż, (8.49)
an equation of the form (6.85). Since y is bounded, so are sr , r and F. Then,
from (8.32), (8.37) and the assumption that J −1 (q) exists, sx and ż must be
bounded, which in turn implies that u is bounded. By applying Lemma 6.1, one
concludes that s and σ must be bounded in D. From (8.9) and (8.26), this implies
that both eo and ėo are bounded. Then, by using (8.5), (8.6), (8.24)–(8.25), one
can rewrite (8.31) as
ẋ + x x = sx + x z − J +
ϕx ξ 2 F − K γ σ . (8.50)
Note that F is bounded because y is bounded, and thus the righthand side
of (8.50) must be bounded. Now, since the lefthand side represents a stable
224 8 Force Control
linear filter, x and ẋ are bounded too. This in turn implies that x and ẋ
are bounded, because xd and ẋd are so. Since q̇ = J(q)−1 ẋ, q̇ must be bounded.
Furthermore, q must be bounded since no singularity has been reached. Consider
now q̇r = J −1 (q)ẋr , which is bounded from (8.27); its derivative is given by

−1 −1 d
q̈r = J̇ (q)ẋr + J (q) Qx (ẍd − x ėo ) − J+
ϕx ξ 2 λ − Kγ σ . (8.51)
dt
Note that from (8.30) it is known that dtd σ is bounded, but it has to be shown that
λ in (8.8) is bounded, where λd is bounded by assumption.The boundedness
of the Lagrange multiplier λ can be shown by using (3.83), i.e.
−1
λ = J ϕ (q)H −1 (q)J Tϕ (q) J ϕ (q)H −1 (q){τ − τ̄ } + J̇ ϕ (q)q̇ ,
where τ̄ = C(q, q̇)q̇ + Dq̇ + g(q). Therefore, λ will be bounded as long as the
input torque τ is bounded because it has been shown that all the other variables are
bounded in D. By taking into account the fact that after (8.31)–(8.32) and (8.36)–
(8.38) one has so = J(q)sr − r, from (8.39) one has
τ = −K p (sr − J −1 (q)r) + J T (q)J Tϕx λd − J T (q)J Tϕx ξ 1 F, (8.52)
which is bounded for y ∈ D. This shows the boundedness of λ, which in turn

implies that of q̈r , meaning that ya in (8.34) is bounded as well. Thus, ṡr in (8.33)
is bounded, and so is ṡx = J̇(q)sr + J(q)ṡr , as a consequence. Now, from (8.43)
one gets
d
ṙ + kd r = ṡx − J +
ϕx ξ 2 λ − K γ σ, (8.53)
dt
which means that ṙ, and thus ẍ in (8.45), must be bounded. Finally, note that
from (8.37) and (8.49), u̇ = ṡx − z̈ is bounded.
(b) The next step is to show that, with a proper choice of gains, one can actually
achieve y < , whenever xd and at least its first and second derivatives are
bounded. Theorem 4.5 is employed as usual. However, this time the ultimate
bound for y given by (4.73) is not important, but rather it is sought that y never
leaves D as long as y(t0 ) satisfies (4.56). Assume that this is the case. If (4.29)
and (4.53) are satisfied in D for some V (y), then y will never leave D and thus
one concludes that y < for all t ≥ t0 as desired. Now define
1 T
V (y) = y My, (8.54)
2

with M = block diag {H(q) I ξ 2 }. Clearly (4.29) is satisfied, i.e.
α1 (y) ≤ V (y) ≤ α2 (y),
with
α1 (y) = λ1 y2 and α2 (y) = λ2 y2 ,
and
1 1
λ1 = min λh , 1, λmin (ξ 2 ) and λ2 = max λH , 1, λmax (ξ 2 ) ,
2 2
where Property 3.7 has been used. After Property 3.2, the derivative of V
along (8.10), (8.40) and (8.45) is given by
V̇ = − sTr K DP sr − sTr J T J Tϕx λ + sTr K p J −1 r

− sTr J T J Tϕx ξ 1 F − sTr ya − kd rT r
+ rT Qx (ẍ + x ėo ) + FT ξ 2 λ. (8.55)
From (8.32) it is sr = J −1 (q)sx ⇒ sTr J T = sTx which combined with (8.31) and
Property 3.16 gives
J ϕx Jsr = J ϕx sx = J ϕx J +
ϕx ξ 2 F + J ϕx K γ σ = ξ 2 F + J ϕx K γ σ , (8.56)
so that one has
V̇ = − sTr K DP sr − σ T K γ J Tϕx λ − FT ξ 2 λ + sTr K p J −1 r

− σ T K γ J Tϕx ξ 1 F − FT ξ 2 ξ 1 F − sTr ya − kd rT r
+ rT Qx (ẍ + x ėo ) + FT ξ 2 λ, (8.57)
or
V̇ = − sTr K DP sr − kd rT r − FT ξ 1 ξ 2 F

− σ T K γ J Tϕx λ + ξ 1 F
− sTr ya + rT Qx (ẍ + x ėo ) + sTr K p J −1 r. (8.58)
While V is positive definite for all y ∈ R(2n+m) , to apply Theorem 4.5 the region
D is necessary to show that (4.53) holds. According to the discussion given in
item a), every signal in D is bounded, so that bounds can be found to satisfy
226 8 Force Control

ᾱ1 = max σ T K γ J Tϕx ξ 1 + ya + Qx (ẍ + x ėo ) (8.59)
∀y∈D

ᾱ2 = max σ T K γ J Tϕx λ (8.60)
∀y∈D

c2 = max J −1 (q). (8.61)
∀y∈D
Then, one gets from (8.58) and Property 3.10
V̇ = − λmin (K p )sr 2 − kd r2

− λmin (ξ 1 ξ 2 )F2
+ ᾱ1 y + ᾱ2 + λmax (K p )c2 sr r, (8.62)
To achieve (4.53) choose
λmin (K p ) = 1 + δ (8.63)
1
kd = δ + λ2max (K p )c22 (8.64)
4
λmin (ξ 1 ξ 2 ) = δ, (8.65)
for some δ > 0, to get

1 1
V̇ ≤ − δy2 − δy2 − ᾱ1 y − ᾱ2 . (8.66)
2 2
A conservative value for μ can be defined as

ᾱ1 + ᾱ12 + 2δ ᾱ2
μ= > 0, (8.67)
δ
so that for y ≥ μ > 0 it is
1
V̇ = − δy2 = −W3 (y). (8.68)
2
Finally, to apply Theorem 4.5 it should be noted that condition (4.55) is satisfied
just by choosing μ arbitrarily small.
(c) Showing that x, ẋ, z, ż and F tend to zero can be done as shown in Step c)
of the proof in Sect. 6.3. Combine From (8.7), (8.9) and (8.31) to get an equation
of the form (6.85), i.e.
u = s + Kγ σ ,
with
u = sx + Qx ż. (8.69)
By computing its derivative and using (8.30) one gets an equation of the
form (6.121), i.e.
ṡ = −K γ K β s − K γ sign(s) + u̇.
Since u̇ is bounded, then by using the same arguments as for Step c) of the proof
in Sect. 6.3 one concludes that in a finite time from (8.9) and (8.24)–(8.25) it
holds
0 = ėo + x eo + J +
ϕx ξ 2 F = sp + sf .
Since sp and sf are orthogonal then
ėo + x eo = 0,
meaning that eo , ėo → 0 while F = 0. But
ėo + x eo = ẋ + x x − ż − x z = 0, (8.70)
so that (8.31) and (8.24)–(8.25) can be rewritten as
sx = ẋ + x x − x z + J +
ϕx ξ 2 F + K γ σ
= ėo + x eo + ż + x z − x z + J +
ϕx ξ 2 F + K γ σ
= r − z z + J +
ϕx ξ 2 F + K γ σ .
So that one concludes that

d
ṡx − ṙ = −z ż + J +
ϕx ξ 2 λ + K γ σ.
dt
On the other hand, from (8.53) it is
d
ṡx − ṙ = kd r + J +
ϕx ξ 2 λ + K γ σ.
dt
Therefore it holds
kd r + z ż = (kd I + z )ż + z z = 0, (8.71)
which represents a stable linear filter for z, meaning that z, ż → 0. But from (8.70)
x, ẋ → 0.
228 8 Force Control
It only remains to show that λ → 0, for which Lemma 4.5 can be used. First
of all, F is bounded and has a limit because F → 0 as t → ∞, so that
in order for λ to tend to zero, it only has to be shown that it is uniformly
continuous, or equivalently, that dtd λ is bounded. But, since λ = λ − λd and
λd and its derivative are assumed to be bounded, it remains only to show that dtd λ
is bounded. From (3.83), it can be seen that the derivative of λ must indeed be
bounded because after the discussion of Step a) all variables have been proven
to be bounded. Therefore, λ → 0.
Finally, force control performance can be improved for unconstrained orientation
by keeping the end-effector perpendicular to the contact surface. The best option is
to employ the unit quaternion. Define
0

ṗd
ẋd = , (8.72)
0
ωd − kε 0 Rd d ε n
t
with xd = t0 ẋd dϑ. kε is a positive scalar gain, 0 pd is the desired position of the end-
effector as before, 0 ωd ∈ R3 is the desired angular velocity, and 0 Rd ∈ R3×3 is the
desired rotation matrix, i.e. it represents the desired orientation. d ε n ∈ R3 is the vector
part of the unit quaternion associated to the rotation matrix given by d Rn = 0 RTd 0 Rn ,
where 0 Rn ∈ R3×3 is the rotation matrix between the end-effector frame and the base
frame. Note that the desired position 0 pd has to be chosen to satisfy (8.1), and that
care should be taken to avoid getting a not bounded xd . Also, the desired orientation
0
Rd has to be chosen perpendicular to the surface at ϕ(0 pd ) = 0. It has already been
shown that x → 0 and ẋ → 0, so that the position error requires no extra analysis,
but the orientation case does. When x = 0, the orientation error dynamics is easily
computed as
ωn + kε 0 Rd d ε n = 0, (8.73)
where ωn = 0 ωn − 0 ωd . The stability analysis can be carried by multiplying (8.73)

by d R0 = 0 RTd style to have the orientation error expressed in the desired coordinated
frame as
d ωn + kε d ε n = 0. (8.74)
Then, it should be noted that the unit quaternion is given by a scalar and a vectorial
part by Q = {d ηn , d n } as explained in Sect. 2.1.4. These should be considered as the
state related to the orientation error whose derivatives can be computed as
1d T d
d
η̇n = − ωn (8.75)
2 n
1 d
d
˙n = ηn I − S d n d ω n , (8.76)
2
by combining (2.75)–(2.77). There is not an unique equilibrium point for (8.75)

and (8.76) but two equilibria points given by
d
Qe1 = ηn = 1, n =0 (8.77)
d
Qe2 = ηn = −1, n =0 . (8.78)
Note that for d n = 0 one has d ωn = 0 after (8.74). Consider the following positive
definite function
d 2
Vε = ηn − 1 + d ε Tn d εn . (8.79)
The derivative of Vε along (8.74) can be computed as

V̇ε = 2 d ηn − 1 d η̇n + 2d εTn d ε̇n

= − d ηn − 1 d Tn d ωn + d ε Tn d ηn I − S d n d ωn

= d Tn d ωn + d ε Tn −S d n d ωn

= −kε d Tn d ε n + d ωTn S d n d ε n
= −kε d n εn ,
Td
(8.80)

where
d itd has been taken advantage of the fact that −ST d n = S d n and that
n = n × n = 0 (see (2.99)). Now, Theorem 4.3 can be used by noting
d d
S n
that V̇ε is negative semidefinite and that V̇ε ≡ 0 in the set M given by the union of
Qe1 and Qe2 , i.e. at the equilibria. This is easy to understand by taking into consid-
eration (2.70), i.e.
η2 + 2
x + 2
y + 2
z = η2 + T
= 1.
Since for d n = 0 one has d ωn = 0, then from (8.75) and (8.76) it is not possible
to know to which equilibrium the trajectory is tending. To get an insight about this
issue, substitute (2.70) in (8.79) to get
d 2
Vε = ηn − 1 + 1 − d ηn2 = 2 1 − d ηn . (8.81)
Substitute now Qe2 to get
Vε = 4. (8.82)
Since η ∈ [−1, 1], then a perturbation σ > 0 can be introduced as d ηn = −1 + σ to

get from (8.81)
Vε = 2(2 − σ ) < 4. (8.83)

230 8 Force Control

Since V̇ε ≤ 0 and Vε d ηn = −1 + σ < Vε d ηn = −1 this means that the trajectory
never returns to Qe2 . In turn this means that the equilibrium Qe1 is asymptotically
stable. Finally, this implies that 0 Rn → 0 Rd and ωn = 0 ωn − 0 ωd → 0.
8.2 Velocity and Force Observers for the Control of Robot

Manipulators
Consider once again the relationship between workspace coordinates and joint coor-
dinates given by (2.175) and (2.183), i.e.
0 0
ṗn ṗn
ẋ = = J(q)q̇ or ẋ = = J(q)q̇, (8.84)
0
φ̇ n 0
ωn
where for the first case J(q) represents the analytic Jacobian and for the second
t one
it stands for the geometric Jacobian. In general, for n = 6 it is 0 φ n = t0 0 ωn dϑ,
but for simplicity’s sake an abuse of notation is made by using (8.2). This is done
because the approach of this section can be employed with any of the two Jacobians
and for which the design of a force control-observer scheme where the desired task
is directly given in workspace coordinates is introduced. Say that Assumption 8.2
holds and use (3.84), i.e.
ϕ(x) = 0,
and model (3.87), i.e.
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ − J T (q)J Tϕx λ,
to get the robot dynamics in workspace coordinates as
H c ẍ + C c ẋ + Dc ẋ + gc = f − J Tϕx λ, (8.85)
where
H c = J −T (q)H(q)J −1 (q) (8.86)

−1

C c = J −T (q) C(q, q̇)J −1 (q) + H(q)J̇ (q) (8.87)
Dc = J −T (q)DJ −1 (q) (8.88)
−T
gc = J (q)g(q) (8.89)
−T
f =J (q)τ , (8.90)
8.2 Velocity and Force Observers for the Control of Robot Manipulators 231
−1
and J̇ (q) = dtd J −1 (q) . The first step is the velocity observer design directly in
workspace coordinates. On the contrary to the observer introduced in the last section,
it is no longer assumed that the contact surface is a plane, but all the errors used before
are employed again, i.e. (8.5)–(8.8). Consider now
x̂ = x̂(t0 ) + ˙
x̂(ϑ) dϑ (8.91)
t0
t
x̂˙ = Qx (ẋd − x eo ) + K d z z(ϑ)dϑ + z z + K d z, (8.92)

t0
where z , K d ∈ Rn×n are diagonal positive definite matrices. At this point, it is still
valid that if Assumption 8.1 holds, then the approximations (8.13) and (8.14) also
hold. However, since the constraint is not necessarily a plane anymore, in order to
make a similar assumption for the observation errors one should have
z ≤ ψ (8.93)
with the same ψ > 0 and make the following assumption.

Assumption 8.3 The robot end-effector begins its movement at the constraint sur-
˙ 0 )) = (x(t0 ), 0).
face at rest so that ẋ(t0 ) = 0 holds, and it is set (x̂(t0 ), x̂(t
Assuming the robot end-effector to be in contact with the environment at t = t0 and
at rest is not unrealistic, but it may require another free movement control algorithm
to take it to that position. Having zero initial observation error guarantees that (8.24)–
(8.25) and
x̂˙ ≈ Qx x̂˙ (8.94)
hold. Certainly, these relationships are in principle only valid at t = t0 , but it will
be shown that it is possible to design a control law to make them valid for all time.
To describe the observer error dynamics, it is taken advantage of a local analysis
where (8.15) and (8.93) hold and it is defined similar to (6.43) and (6.45) the following
variables
ẋo = x̂˙ − z z (8.95)

r = ẋ − ẋo = ż + z z. (8.96)
Now, rewrite (8.92) as

t
ẋ − x̂˙ + K d z z(ϑ)dϑ + z z + K d z = Qx (ẋ − ẋd + x eo ) , (8.97)

t0
232 8 Force Control
where (3.120) has been used. Then compute the derivative to get
ṙ + K d r = Qx (ẍ + x ėo ) + Q̇x (ẋ + x eo ) . (8.98)
For control design assume that the vector of Lagrange multipliers λ ∈ Rm is not
measurable so that an estimated value has to be computed as λ̂ ∈ Rm . Of course, the
regular force errors (8.8) and (8.10) can no longer be used and the following auxiliary
errors have to be introduced
λ̄ = λ̂ − λd , (8.99)
and
t
F̄ = λ̄dϑ, (8.100)

t0
where λd ∈ Rm is the desired bounded force, with at least its first derivative bounded.
Instead of (8.9) one has
s = Qx (ėo + x eo ) + J +
ϕx ξ 2 F̄ = sp + sf , (8.101)
but as before sp and sf are orthogonal vectors, and they both must become zero if s
vanishes. In that case, since J Tϕx and J +
ϕx are assumed to be full rank, one has
s = 0 ⇒ sf = 0 ⇒ F̄ = 0, (8.102)
because ξ 2 ∈ Rm×m is a positive definite diagonal matrix. On the other hand, note
that by assuming as before Qx x = x Qx (e.g. by setting x = kx I, with kx > 0),
then in view of (8.13) and (8.14) it is
sp = Qx (ėo + x eo ) ≈ ėo + x eo . (8.103)
From (8.103) one can conclude that both eo and ėo tend to zero if sp → 0, i.e.
if s → 0. Furthermore, consider once again (8.28), or (8.30), and alike to (8.27)
and (8.31) define
ẋr = Qx (ẋd − x eo ) − J +
ϕx ξ 2 F̄ − K γ σ (8.104)
sx = ẋ − ẋr = Qx (ẋ + x eo ) + J+
ϕx ξ 2 F̄ + Kγ σ . (8.105)
This allows to rewrite the robot dynamics in (8.85) as follows
H c ṡx + C c sx + Dc sx = f − J Tϕx λ − ya , (8.106)

where ya = H c ẍr + C c ẋr + Dc ẋr + gc . Based on all previous definitions, the pro-
posed control law is
so = ẋo − ẋr (8.107)

f = −K p so + J Tϕx λd − ξ 1 F̄ − ξ 3 λ̄ , (8.108)
with ξ 1 , ξ 3 ∈ Rm×m diagonal positive definite matrices. Note that for implementation
it must be employed
τ = J T (q)f (8.109)
after (8.90). By substituting (8.108) in (8.106) and by noting that so = sx − r, one

gets

H c ṡx + C c sx + K DP sx = K p r − J Tϕx λ + ξ 1 F̄ + ξ 3 λ̄ − ya , (8.110)
where K DP = Dc + K p . Equation (8.110) describes the dynamics of tracking errors.

It remains to design the force observer. In order to do so, from (3.84) it is
ϕ̈(x) = J ϕx ẍ + J̇ ϕx ẋ = 0, (8.111)
so that by taking into account (8.85) it is

T −1

λ = J ϕx H −1
c J ϕx J ϕx H −1
c f − C c ẋ − Dc ẋ − gc + J̇ ϕx ẋ . (8.112)
An observer is then given by

T −1

λ̂ = J ϕx H −1 J J ϕx H −1
f − C (x, ˙ x̂˙ − Dc x̂˙ − g + J̇ ϕx (x, x̂)
x̂) ˙ .
c ϕx c c c
(8.113)
Note that C c becomes dependent on x̂˙ through q̂˙ = J −1 (q)x̂.

˙ By substituting λ̄ =
λ̂ − λd and (8.108), λ̂ can be implemented as

T −1
λ̂ = λd − (I + ξ 3 )−1 ξ 1 F̄ + J ϕx H −1 c J ϕx f x (8.114)

f x = J ϕx H −1 K p so + ˙
C c x̂˙ + Dc x̂˙ + gc − Ĵ ϕx x̂,
˙ (8.115)
c
where it has been defined
˙
C c = C c (x, x̂) (8.116)
Ĵ˙ ϕx = J̇ ϕx (x, x̂).
˙ (8.117)
234 8 Force Control
Then, the state of the errors dynamics in (8.98), (8.100) and (8.110) can be defined
as
⎡ ⎤
sx
y = ⎣ r ⎦. (8.118)
F̄
As usual define a region of interest of the form (4.28), i.e.

D = y ∈ R(2n+m) |y < ,
but with the constraint that cannot be made arbitrary large. On the contrary, it
has to be chosen small enough for (8.15) and (8.93) to hold. Recall that for that
it is mandatory to choose xd satisfying (3.84) but also far away enough from any
singularity and with bounded first and second derivatives. In the same fashion, the
desired bounded force λd must be bounded as well as its first derivative.
To employ Theorem 4.5, consider the following positive definite function for y
in (8.118)
1 T
V (y) = y My, (8.119)
2
with M = block diag {H(q) I ξ 2 ξ 3 }. Clearly, it satisfies (4.29), i.e.
α1 (y) ≤ V (y) ≤ α2 (y),
with
α1 (y) = λ1 y2 and α2 (y) = λ2 y2 ,
and
1 1
λ1 = min λh , 1, λmin (ξ 2 ξ 3 ) and λ2 = max λH , 1, λmax (ξ 2 ξ 3 ) ,
2 2
where Property 3.7 has been used.
As done in the previous section, three steps are necessary to prove local asymptotic
stability in D.
(a) First of all, it can be shown that whenever y is bounded by 0 < < ∞, then any
other signal related to the closed loop system is bounded. This step is ommited
because it is completely similar to that shown in the previous section.
(b) The next step is to set gains to enforce y < for all t ≥ t0 . This can be done
by using Theorem 4.5. By direct computation the derivative of V in (8.119)
along (8.98), (8.100) and (8.110) is given by
1 T
V̇ = sTx Ḣ c sx + sTx H c ṡx + rT ṙ + F̄ ξ 2 ξ 3 λ̄
2
= − sTx K DP sx + sTx K p r − sTx ya − sTx J Tϕx λ − sTx J Tϕx ξ 1 F̄
− sTx J Tϕx ξ 3 λ̄ + rT Qx (ẍ + x ėo ) + rT Q̇x (ẋ + x eo )
T
− rT K d r + F̄ ξ 2 ξ 3 λ̄. (8.120)
From (8.105) it is
T
sTx J Tϕx = F̄ ξ 2 + σ T K γ J Tϕx , (8.121)

T −1
because after Property 3.16 it is J ϕx J +
ϕx = J ϕx J ϕx J ϕx J ϕx
T
= I and J ϕx Qϕx =
O. By substituting (8.121) in (8.120)
T
V̇ = − sTx K DP sx + sTx K p r − sTx ya − rT K d r + F̄ ξ 2 ξ 3 λ̄
T T
− F̄ ξ 2 λ − σ T K γ J Tϕx λ − F̄ ξ 1 ξ 2 F̄ − σ T K γ J Tϕx ξ 1 F̄
T
− F̄ ξ 2 ξ 3 λ̄ − σ T K γ J Tϕx ξ 3 λ̄
+ rT Qx (ẍ + x ėo ) + rT Q̇x (ẋ + x eo ) . (8.122)
While V is positive definite for all y ∈ R(2n+m) , to apply Theorem 4.5 it is

necessary to show that (4.53) holds in D. But, according to the discussion given
in Step a), every signal in D is bounded, so that bounds can be found to satisfy

ᾱ1 = max ya (8.123)
∀y∈D

ᾱ2 = max Qx (ẍ + x ėo ) + Q̇x (ẋ + x eo ) (8.124)
∀y∈D

ᾱ3 = max σ T K γ J Tϕx ξ 1 + λT ξ 2 (8.125)
∀y∈D

ᾱ4 = max σ T K γ J Tϕx (ξ 3 λ̄ + λ). (8.126)
∀y∈D
Keeping definitions (8.123)–(8.126) in mind, one gets from (8.122)
V̇ = − λmin (K p )sx 2 − λmin (K d )r2 − λmin (ξ 1 ξ 2 )F̄2

+ ᾱ1 sx + ᾱ2 r + ᾱ3 F̄
+ ᾱ4 + λmax (K p )sx r
= − λmin (K p )sx 2 − λmin (ξ 1 ξ 2 )F̄2 − λmin (K d )r2
+ αy + ᾱ4 + λmax (K p )sx r, (8.127)
236 8 Force Control

where α = ᾱ1 + ᾱ2 + ᾱ3 . To achieve (4.53), gains can be chosen as
λmin (K p ) = 1 + δ (8.128)
1
λmin (K d ) = δ + λ2max (K p ) (8.129)
4
λmin (ξ 1 ξ 2 ) = δ, (8.130)
with δ > 0, to get
V̇ = −δsx 2 − δr2 − δF2 + αy + ᾱ4

1 1
= − δy2 − δy2 − αy − ᾱ4 . (8.131)
2 2
A conservative value for μ in (4.53) can be found to be

α+ α 2 + 2δ ᾱ4
μ= > 0, (8.132)
δ
so that if y ≥ μ one has
1
V̇ = − δy2 = −W3 (y). (8.133)
2
Condition (4.55), i.e.
μ < α2−1 (α1 ( )),
can be achieved by tuning ξ 1 , ξ 2 , K p and K d large enough. Finally, by assuming

that condition (4.56) holds, i.e.
y(t0 ) = y0 ≤ α2−1 (α1 ( )),
then it can be concluded that y ∈ D for all t ≥ t0 , which allows to conclude this
part of the proof.
(c) Till now it has been proven that y is bounded by as long as the initial condition
is small enough and gains are properly set. Showing that F̄, x and ẋ, z, ż, x
and ẋ tend to zero can be done as shown in Step c) of the proof of the previous
section. To show that λ̄ → 0, once again Lemma 4.5 can be used. First of all,
it is known that F̄ is bounded and has a limit (F̄ → 0) as t → ∞. Then, in
order for λ̄ to tend to zero, dtd λ̄ must be bounded. Since λ̄ = λ̂ − λd and
λd and its derivative are assumed to be bounded, it remains only to show that
d
dt
λ̂ is bounded. From (8.100) and (8.114) one has
d d
λ̂ = λd − (I + ξ 3 )−1 ξ 1 λ̄+
dt dt
d −1 T −1

−1 T −1 d
J ϕx H c J ϕx f x + J ϕx H c J ϕx f . (8.134)
dt dt x
From Step a), the right hand side of (8.134) must be bounded. This in turn means
that dtd λ̂ and dtd λ̄ are bounded. After the aforementioned lemma λ̄ tends to
zero. On the other hand, since x̂˙ → ẋ one must have from (8.112) and (8.113)
that λ̂ → λ. Finally, this necessarily means that λ = λ − λd → 0.
8.3 GPI Based Velocity/Force Observer Design for Robot

Manipulators
As it was shown in Sect. 7.5 a possibility to deal with uncertain model parameters
are the Generalized Proportional Integral (GPI) observers. Consider model (3.77),
i.e.
H(q)q̈ + C(q, q̇)q̇ + Dq̇ + g(q) = τ − J Tϕ (q)λ,
where J ϕ (q) = ∇ϕ(q) ∈ Rm×n as given in (3.78) with respect to the holonomic
constraint given in (3.66), i.e.
ϕ(q) = 0 ,
which arises when the manipulator is in contact with the environment. Therefore,
J ϕ (q) maps a vector into the normal plane at the contact point. It is assumed that
J ϕ (q) is full rank, i.e. rank(J ϕ (q)) = m. Note that usually J ϕ (q) can be normalized
to get
J ϕ = 1. (8.135)
In the following it will be assumed that this is the case. If the position tracking error
is defined by
e = q − qd = Q(q)q + P(q)q − Q(qd )qd − P(qd )qd , (8.136)
where qd fulfils (3.66), then completely similar to (8.13), if e is small enough it holds
e ≈ Q(q)(q − qd ) = Q(q)e, (8.137)
because the error tends to be tangent to the surface. But in fact Property 3.16 is valid
also for joint space coordinates, i.e. q̇ = Q(q)q̇ so that it holds as well
238 8 Force Control
q̇d ≈ Q(q)q̇d ⇒ ė ≈ Q(q)(q̇ − q̇d ) = Q(q)ė. (8.138)
Indeed, “small enough” does not necessarily mean e ≈ 0, so that it is more convenient
to make the following assumption equivalent to (8.15).
Assumption 8.4 The approximations (8.137) and (8.138) are valid if
e ≤ ψ (8.139)
holds for some ψ > 0.
In order to design a velocity/force observer for tracking control, first consider

rewriting (3.77) as
!
q̈ = H −1 (q) τ − C(q, q̇)q̇ − Dq̇ − g(q) − H −1 (q)J Tϕ (q)λ. (8.140)
Define
z1 = −H −1 (q)J Tϕ (q)λ (8.141)

N(q, q̇) = C(q, q̇)q̇ + Dq̇ + g(q), (8.142)
to have
!
q̈ = H −1 (q) τ − N(q, q̇) + z1 . (8.143)
Just like in the previous section, in order to design a force control law it will be
necessary to estimate both the force λ and the state q̇. However, this time the same
approach as that of Sect. 7.5 will be employed, for which it is necessary to assume
that the vector z1 can be expressed as in (7.110), i.e.
"
p−1
z1 (t) = ai t i + r(t) , (8.144)
i=0
where each ai is a n–vector of constant coefficients and r(t) represents a residual

term whose first p time derivatives are assumed to exist. This is necessary to rewrite
z1 (t) in the following form
ż1 = z2 (8.145)
ż2 = z3
..
.
ż p−1 = z p
ż p = r( p) (t) . (8.146)
8.3 GPI Based Velocity/Force Observer Design for Robot Manipulators 239
On the contrary to Sect. 7.5, the following GPI observer directly estimates velocities
instead of errors
q̂˙ = q̂2 + λ p+1 ẽ (8.147)

!
q̂˙ 2 = H −1 (q) τ − N(q, q̂2 ) + ẑ1 + λ p ẽ (8.148)
ẑ˙ 1 = ẑ2 + λ p−1 ẽ (8.149)
ẑ˙ 2 = ẑ3 + λ p−2 ẽ
..
.
ẑ˙ p−1 = ẑp + λ1 ẽ (8.150)
ẑ˙ p = λ0 ẽ, (8.151)
where
ẽ = q − q̂ . (8.152)
Also, q̂2 is an estimated value for q̇. In order to compute the system closed loop
dynamics, consider the following error definitions
z̃k = zk − ẑk , (8.153)
for k = 1, . . . , p. Then, from (8.143), (8.145)–(8.146) and (8.147)–(8.151) one gets

directly
ẽ˙ = q̇ − q̂˙ = ẽ2 − λ p+1 ẽ (8.154)

!
ẽ˙ 2 = −H (q) N(q, q̇) − N(q, q̂2 ) + z̃1 − λ p ẽ
−1
(8.155)
z̃˙ 1 = z̃2 − λ p−1 ẽ (8.156)
z̃˙ 2 = z̃3 − λ p−2 ẽ
..
.
z̃˙ p−1 = z̃p − λ1 ẽ
z̃˙ p = r( p) (t) − λ0 ẽ, (8.157)
where
ẽ2 = q̇ − q̂2 . (8.158)
It is rather direct to get from (8.156)–(8.157) the following relationship

( p)
z̃1 = r( p) (t) − λ0 ẽ − · · · − λ p−1 ẽ( p−1) , (8.159)
240 8 Force Control
while from (8.142) and (8.155) it is
N(q, q̇) − N(q, q̂2 ) = C(q, q̇)q̇ − C(q, q̂2 )q̂2 + Dẽ2 . (8.160)
Combining (8.154) and (8.158) it is
ẽ2 = q̇ − q̂2 = ẽ˙ + λ p+1 ẽ, (8.161)
which implies that
q̂2 = q̇ − ẽ˙ − λ p+1 ẽ. (8.162)
By applying Property 3.4 and after a rather direct computation one gets
C(q, q̂2 )q̂2 = C(q, q̇ − ẽ˙ − λ p+1 ẽ)(q̇ − ẽ˙ − λ p+1 ẽ)
= C(q, q̇)q̇ − 2C(q, q̇)(ẽ˙ + λ p+1 ẽ) + C(q, ẽ˙ + λ p+1 ẽ)(ẽ˙ + λ p+1 ẽ),
(8.163)
which means that by taking this relationship and (8.161) into account, (8.160) can
be rewritten as
N(q, q̇) − N(q, q̂2 ) =2C(q, q̇)(ẽ˙ + λ p+1 ẽ)

− C(q, ẽ˙ + λ p+1 ẽ)(ẽ˙ + λ p+1 ẽ) + D(ẽ˙ + λ p+1 ẽ), (8.164)
which in turn means that (8.155) can be expressed as

ẽ˙ 2 = z̃1 − λ p ẽ − H −1 (q) 2C(q, ė + q̇d )(ẽ˙ + λ p+1 ẽ)

− C(q, ẽ˙ + λ p+1 ẽ)(ẽ˙ + λ p+1 ẽ) + D(ẽ˙ + λ p+1 ẽ) , (8.165)
where the equality q̇ = ė + q̇d has been employed. Combine with (8.161) to have
ẽ¨ + λ p+1 ẽ˙ + λ p ẽ = z̃1 + f (t), (8.166)
with

f (t) = −H −1 (q) 2C(q, ė + q̇d )(ẽ˙ + λ p+1 ẽ)

−C(q, ẽ˙ + λ p+1 ẽ)(ẽ˙ + λ p+1 ẽ) + D(ẽ˙ + λ p+1 ẽ) . (8.167)
Finally, by computing the p-th derivative of (8.166) and substituting in (8.159) one
gets
ẽ( p+2) + λ p+1 ẽ( p+1) + · · · + λ0 ẽ = r( p) (t) + f ( p) (t). (8.168)
Equation (8.168) represents the observer closed loop dynamics and it can be rewritten
in the form (7.133)–(7.136) as
ẋo = Axo + Brf , (8.169)
with rf = r( p) + f ( p) and
⎡ ⎤
ẽ
⎢ .. ⎥
xo = ⎣ . ⎦ (8.170)
( p+1)
ẽ
⎡ ⎤
O I ··· O
⎢ .. .. ⎥
⎢ . ⎥
A=⎢ . ⎥ (8.171)
⎣ O O ··· I ⎦
−λ0 −λ1 · · · −λ p+1
!T
B = O ··· O I . (8.172)
The next step is to design the force observer. Consider that after (8.141) it is
J Tϕ (q)λ = −H(q)z1 . (8.173)
Therefore, λ can be obtained by doing
λ = (J +
ϕ ) H(q)z1 ,
T
(8.174)
because J ϕ is full rank. But since an estimated value of z1 is known, then it is quite
direct to get an estimated value for λ as
λ̂ = (J +
ϕ ) H(q)ẑ1 ,
T
(8.175)
which means that the force observation error is given by
λ̃ = λ − λ̂ = (J + + T
ϕ ) H(q)z1 − (J ϕ ) H(q)ẑ1
T
= (J +
ϕ ) H(q)z̃1 ,
T
(8.176)
where z̃1 = z1 − ẑ1 is the observation error for z1 . Note that as mentioned before it
is assumed that J ϕ = 1 and that the robot pushes on the surface so that the force
242 8 Force Control
is always positive or zero. For that case, when the constraint in (3.66) has dimension
one, i.e. m = 1, so that λ̂ is a scalar value, then it holds
λ̂ = J ϕ λ̂ = H(q)ẑ1 . (8.177)
Finally, the control law is chosen as a simple PID given by
τ = − K v (q̂2 − q̇d ) − K p e − K i Q e dϑ
t0
− J Tϕ (q)λd + kFi J +
ϕ (q)F̄ , (8.178)
where K v , K p ∈ Rn×n are positive definite diagonal matrices and as usual (·)d denotes
the desired value of (·), which is assumed to be bounded, as well as its derivatives,
both for forces and positions. Also, (8.99) and (8.100) are used again, i.e.
λ̄ = λ̂ − λd
t
F̄ = λ̄ dϑ .
t0
To compute the closed loop system dynamics consider first rewriting (8.178) as
τ = − K v (ė − ẽ˙ − λ p+1 ẽ) − K p e − K i Q e dϑ

t0
− J Tϕ (q)λd + kFi J +
ϕ (q)F̄
t
= − K v ė − K p e − K i Q e dϑ
t0
− J Tϕ (q)λd + kFi J + ˙
ϕ (q)F̄ + K v (ẽ + λ p+1 ẽ), (8.179)
where (8.162) was taken into account. Define the auxiliary variable
ẇ = ė + e (8.180)
with ∈ Rn×n a diagonal positive definite matrix. As before, the goal is to carry
out a local stability analysis where (8.139) is valid, so that e = Qe and ė = Qė hold.
Furthermore, by assuming Q = Q (e.g. by setting = kλ I, with kλ > 0) then
also holds ẇ = Qẇ, so that the control law in (8.179) is equivalent to
τ = −K v ẇ − K̄ i Qw − J Tϕ λd + kFi J + ˙
ϕ (q)F̄ + K v (ẽ + λ p+1 ẽ), (8.181)
with
K p = K v + K̄ i (8.182)
K i = K̄ i . (8.183)
By substituting (8.181) in (3.77) one gets
1
H(q)q̈ = − C(q, q̇)q̇ − Dq̇ + g(q) − K v ẇ − K̄ i Qw + J Tϕ λ
2
1 1 1 ˙
+ J Tϕ λ + kFi J + +
ϕ F̄ + kFi J ϕ F̄ + K v (ẽ + λ p+1 ẽ), (8.184)
2 2 2
where λ = λ − λd is the force error as given in (8.8). The previous expression may
not be adequate to carry out the stability analysis, but consider defining
1 −1 T 1
q̇r = q̇d − e − K −1 −1 +
v K̄ i Qw + K v J ϕ λ + kFi K v J ϕ F̄ (8.185)
2 2
s = q̇ − q̇r

−1 1 −1 T 1 −1 +
= (ẇ + K v K̄ i Qw) + − K v J ϕ λ − kFi K v J ϕ F̄
2 2
= sp + s f . (8.186)
It is desired as before that sp and sf lie in orthogonal subspaces. To achieve this it is

necessary that PK −1 −1 T
v J ϕ = K v J ϕ holds. This can be done by setting K v = kv I, with
T
kv > 0. Therefore, one can rewrite (8.184) as
1 T 1
H(q)ṡ + C(q, q̇)s + K d s = J ϕ λ + kFi J +
ϕ F̄ + ya , (8.187)
2 2
where K d = D + K v and

ya = K v (ẽ˙ + λ p+1 ẽ) − H(q)q̈r + C(q, q̇)q̇r + Dq̇r + g(q) . (8.188)
Then, the complete closed loop system dynamics is described by equations (8.100),
(8.169) and (8.187), for which the corresponding state vector is defined as
⎡ ⎤
xo
x = ⎣ s ⎦. (8.189)
F̄
For this state it is always possible to define a region of the form (4.28), i.e.

D = x ∈ Rn( p+2)+m |x < ,
244 8 Force Control
where is a positive constant chosen small enough for (8.15) to hold. Then, it
can be shown that for qd a desired bounded trajectory fulfilling constraint (3.66)
with at least their first p + 2 derivatives bounded and a bounded desired force λd
whose derivatives up to p + 2 are also bounded, a combination of gains always exist
to make the tracking errors (e, ė), the observation errors xo , and the force errors
(λ, λ̃, λ̄, F̄) arbitrarily small as long as the initial condition x(t0 ) lies in a
subset of D chosen small enough to avoid the trajectory x leaving D during the
transient response. Furthermore, the estimate ẑ1 becomes arbitrarily close to the real
value z1 . The proof is omitted because it is rather involved and not directly based in
the theory presented in Chap. 4. Note that it may be required that the initial velocity
be zero because for that case the initial observation and tracking errors can easily be
set to zero.
In this section, an experimental evaluation of the force controllers described above is

presented. The experimental setup consisted of the Geomagic Touch robot described
in Chap. 14 in contact with a horizontal-flat surface, and the ATI Nano17 force sensor
attached at the tip of the manipulator. The three degrees of freedom configuration
characterized in Sect. 14.2.5 is employed, i.e. the last three joints are mechanically
blocked.
The desired position trajectory for all the experiments below is given by
⎡ ⎤
0.12 m
pd = ⎣0.02 sin (0.25π t) m⎦ ,
−0.1425 m
i.e. a sinusoidal over the Cartesian axis y0 . The reason to select such trajectory is
the limitation imposed by keeping the end-effector perpendicular to the surface,
when using the three-degrees-of-freedom configuration of the Phantom robot. The
desired Cartesian velocities are obtained by taking the time-derivative of the desired
positions. In turn, the desired force is defined by
λd = 0.5 − 0.3 cos (π t) N .
Robot force control without dynamic model

Tuning guide
The equations required to implement the scheme of Sect. 8.1 are the tracking and
observation errors (8.5)–(8.7), the force error definitions (8.8) and (8.10), the velocity
observer (8.23), the auxiliary variables (8.9), (8.26)–(8.28) and (8.36)–(8.38), and
the control law (8.39). The tuning of this controller-observer can be carried out as
follows.
Table 8.1 Chosen gains for the force control without dynamic model scheme of Sect. 8.1
Gain Value
Kp 0.2I
x 20I
kd 10
z 10I
Kβ 5I
Kγ 0.1I
ζ1 0.5I
ζ2 I
• At first, neglect the force control terms as well as the sliding mode ones, by setting
λd = 0 and ζ 1 = ζ 2 = K β = K γ = O.
• Tune the observer gains, kd and z , satisfying (6.65)–(6.67) and (6.69)–(6.70), to
• Tune the remaining position control tracking as a standard PD controller, with
K p as the derivative gain and x = K −1 p K px , where K px is the proportional gain
matrix.
• Increase K γ slowly to improve the tracking performance, but preventing the actu-
ators to present chattering.
• Increase K β slowly in the same manner.
• Include the force control terms and increase ζ 1 and z2 until obtaining the desired
force tracking performance.
Experiment
The selected gains for the implementation of the controller presented in Sect. 8.1, by
following the tuning guide above are displayed in Table 8.1. The Cartesian position
tracking is shown in Fig. 8.3, whereas the corresponding tracking error is displayed
in Fig. 8.4. The Cartesian velocities estimated by the proposed observer, displayed
along with the desired velocities, are presented in Fig. 8.5. In Fig. 8.6, the force
tracking and the force tracking error are shown as well.
Velocity and Force Observers
Tuning Guide
The equations needed to implement the controller based on the velocity and force
observers of Sect. 8.2 are the tracking and observation errors (8.5)–(8.7), the force
error definitions (8.8) and (8.10), the force observation errors (8.99)–(8.100), the
velocity observer (8.92), the dynamics in workspace coordinates (8.86)–(8.90), the
auxiliary variables (8.101), (8.103)–(8.105), and (8.107), the control law (8.108)–
(8.109), and the force observer (8.114). The tuning of this controller-observer can
be carried out as follows.
246 8 Force Control
Fig. 8.3 Robot force control without dynamic model, Cartesian position tracking: desired (· · · ),
real (—)
λd = 0 and ζ 1 = ζ 2 = ζ 3 = K β = K γ = O.
• Tune the remaining position tracking control as a standard PD controller, with
K p as the derivative gain and x = K −1 p K px , where K px is the proportional gain
matrix.
• Increase K γ slowly to improve the tracking performance, but preventing the actu-
ators to present chattering.
• Increase K β slowly in the same manner.
• Include the force control terms and increase ζ 1 and z2 until obtaining the desired
• Finally, increase ζ 3 slowly to improve the force tracking. Note that ζ 3 = I
for (8.114) to be well defined.
Experiment
The selected gains for the implementation of the controller presented in Sect. 8.2, by
following the tuning guide above are displayed in Table 8.2.
Fig. 8.4 Robot force control without dynamic model, Cartesian position tracking error
Remark 8.1 Because of the extra weight added by the force sensor, the parameters
obtained in Sect. 14.2.5 are no longer valid. Instead, a new identification was carried
out following the same methods of Sect. 14.2.5, giving the new parameters vector
!T
θ 0 = 0.0011 0.0044 0.0045 0.0457 0.0831 0.0771 0.0125 0.0242 .

The Cartesian position tracking is shown in Fig. 8.7, whereas the corresponding
tracking error is displayed in Fig. 8.8. The Cartesian velocities estimated by the pro-
posed observer, displayed along with the desired velocities, are presented in Fig. 8.9.
The force tracking and force estimation, as well as the corresponding force tracking
error and force observation error, are shown in Fig. 8.10.
248 8 Force Control
Fig. 8.5 Robot force control without dynamic model, Cartesian velocities: desired (· · · ),
observer (—)
Table 8.2 Chosen gains for the velocity and force observers based controller of Sect. 8.2
Gain Value
Kp 20I
x 20I
kd 10
z 10I
Kβ 5I
Kγ 0.1I
ζ1 0.5I
ζ2 I
ζ3 0.01I
Fig. 8.6 Robot force control without dynamic model, contact forces (up) and force error (down):
desired (· · · ), measured (—)
GPI based velocity and force observers

Tuning guide
The required equations to implement the GPI force/velocity observers controller of
Sect. 8.3 are the tracking error (8.136), the observer equations (8.147)–(8.151), the
observation errors (8.152) and (8.153), the force error definitions (8.8) and (8.10), the
force observation errors (8.99)–(8.100), the force observer (8.175), and the control
law (8.178). This controller can be tuned by following the suggestions below.
• Choose the degree of the polynomial, p, in (8.144) to define the order of the state
extension, which in turn is the internal model for the unknown signals. As a rule,
the higher this value, the more accurate will be the estimation of the unknown
terms lumped in z1 , but the peaking phenomena is worsen. For most manipulation
tasks, p = 2 is a good choice.
• Compute the matrix gains λ0 in (8.168), by defining the poles of the polyinomial
matrix in the left-hand side of this equation. Locate the poles as far on the open
left-half complex plane as the bandwidth of the system allows it. As a rule, the
more on the left the poles are located, the more accurate the observer estimation,
but at the expense of augmenting the peaking phenomena and the sensitivity to
noise.
250 8 Force Control
Fig. 8.7 Velocity and force observers, Cartesian position tracking: desired (· · · ), real (—)
• Now, neglect temporarily the force control terms by making λd = 0 and kFi = 0,
and tune the remaining ones as a standard PID controller to get the desired position
tracking performance.
• Finally, include the force control terms and increase slowly kFi to obtain the desired
Remark 8.2 Note that since this controller is designed in joint coordinates, the
inverse and differential kinematics are required for the implementation.
Remark 8.3 Due to the high gain nature of GPI observers, sometimes it may be
advisable to filter ẑ1 in the following form
żf = −ωf zf + ωf ẑ1 , (8.190)
with ωf > 0. In that case the filtered estimated force is given by
λ̂f = (J +
ϕ ) H(q)zf .
T
(8.191)

Fig. 8.8 Velocity and force observers, Cartesian position tracking error
Remark 8.4 To reduce the undesired peaking phenomena, it is convenient to imple-

ment a suitable clutch, which can be of the form:
#
sin8 ( πt ) if t ≤
f clutch = 2 ,
1 if t >
where is the time duration of the peaking. Finally, multiply ê2 and ẑi , i = 1, . . . , p,
by f clutch .
Experiment
The gains chosen for the implementation of the GPI force/velocity observers based
force controller of Sect. 8.3, by following the tuning guide above, are shown in
Table 8.3. The Cartesian position tracking is shown in Fig. 8.11, whereas the corre-
sponding tracking error is displayed in Fig. 8.12. The Cartesian velocities estimated
by the proposed observer, displayed along with the desired velocities, are presented
in Fig. 8.13. The force tracking and force estimation, as well as the corresponding
force tracking error and force observation error, are shown in Fig. 8.14.
252 8 Force Control
Fig. 8.9 Velocity and force observers, Cartesian velocities: desired (· · · ), observer (—)
Fig. 8.10 Velocity and force observers, contact forces (up), force tracking error (middle), and force
observation error (down): desired (· · · ), measured (—), observer (- . -)
Table 8.3 Chosen gains for the GPI force/velocity observers based controller of Sect. 8.3
Gain Value
λ0 1 × 108 I
λ1 4 × 106 I
λ2 6 × 104 I
λ3 400I
Kp 5I
Ki 0.1I
Kv 0.05I
kFi 0.5
0.8 s
Fig. 8.11 GPI velocity/force observers, Cartesian position tracking: desired (· · · ), real (—)
254 8 Force Control
Fig. 8.12 GPI velocity/force observers, Cartesian position tracking error
Fig. 8.13 GPI velocity/force observers, Cartesian velocities: desired (· · · ), observer (—)
References 255
Fig. 8.14 GPI velocity/force observers, contact forces (up), force tracking error (middle), and force
observation error (down): desired (· · · ), measured (—), observer (- . -)
References
Arimoto S (1995) Joint-space orthogonalization and passivity for physical interpretations of dex-
terous robot motions under geometric constraints. Int J Robust Nonlinear Control 5(4):259–284
Arteaga-Pérez MA, Rivera-Dueñas JC (2007) Force control without inverse kinematics nor robot
model. In: Proceedings CD ROM. European Control Conference ECC07. Kos Island, Greece, pp
4385–4392
Arteaga-Pérez MA, Rivera-Dueñas JC, Gutiérrez-Giles A (2013) Velocity and force observers for
the control of robot manipulators. ASME J Dyn Syst Meas Control 135:064502.1–064502.7
Gutiérrez-Giles A, Arteaga-Pérez MA (2014) GPI based velocity/force observer design for robot
manipulators. ISA Trans 53(4):929–938
Rivera-Dueñas JC, Arteaga-Pérez MA (2013) Robot force control without dynamic model: theory
and experiments. Robotica 31:149–171
Siciliano B, Villani L (1999) Robot force control. Kluwer Academic Publishers, The Netherlands
Slotine JJE, Li W (1991) Applied nonlinear control. Prentice-Hall, Englewood Cliffs, NJ
Chapter 9
Bilateral Teleoperation
Abstract Bilateral local-remote, or master-slave, teleoperation systems make a

combination of human skills with the benefits of precise, repetitive and cost-effective
robotic manipulation. Telepresence, task performance and transparency should be
optimized. A good and robust performance are ideal characteristics because the
human operator may use the bilateral system without any special training. The pres-
ence of time varying delays makes the achievement of these objectives not possible
and some new performance parameters should be introduced.
9.1 Fundamental Concepts of Bilateral Manipulators

Systems
Figure 9.1 shows the five basic elements of a bilateral teleoperation system. The first
component is a human being, the operator, who directly interacts with the second
element, the local or master manipulator. The third component is a communication
channel for the exchange of different signals, among other positions, velocities,
forces, video, etc. The fourth element is the remote or slave robot which interacts
with the fifth component, the environment. It is not difficult to get a dynamic model
for the system depicted in Fig. 9.1 based on model (3.44), where the notation local
(l) and remote (r) is preferred over the common one of master-slave. It is assumed
that both manipulators have n DOF and the same kinematic configuration, although
a scale factor might just be necessary if they have different sizes.
H l (ql )q̈l + C l (ql , q̇l )q̇l + Dl q̇l + gl (ql ) = τ l − τ h (9.1)

H r (qr )q̈r + C r (qr , q̇r )q̇r + Dr q̇r + gr (qr ) = τ r + τ e . (9.2)
τ h ∈ Rn represents the torque applied by the human operator to the local robot and
τ e ∈ Rn is the environment interaction.
Figure 9.1 also shows a particular case, i.e. when time varying delays are present
in the communication channel. Note that not necessarily all the signals shown in the
https://doi.org/10.1007/978-3-030-85980-0_9
258 9 Bilateral Teleoperation
Fig. 9.1 Components of a

bilateral teleoperation system
figure are usually transferred. For instance, velocities are most likely not available.
This does not only make the control, and perhaps the observer, design more complex
because its stability is difficult to be proven analytically, but it makes less clear what
are the main objectives to pursue. And that for a good reason. The usual concepts
of telepresence and transparency are defined for systems without communication
delays. The former is rather a subjective objective that means that the operator has the
feeling of being in the remote environment, while the latter implies that the physical
medium between he/she and the environment does not impose any dynamics, i.e. that
local and remote robots dynamics are cancelled out. The concept of transparency is
not only fundamental, but also objectively quantifiable in two usual ways. The first
one is the following:
Criterion Transparency based on impedance
1. The impedance felt by the operator, say Zh , must be equal to the task impedance,
say Ze , i.e. Zh ≡ Ze , where the following relationships hold τ h (s) = Zh (q̇l (s))
and τ e (s) = Ze (q̇r (s)) as well.
2. Velocities must be equal, i.e. q̇l (t) = q̇r (t).
9.1 Fundamental Concepts of Bilateral Manipulators Systems 259
A second possibility is to define three ideal responses for kinematic correspon-

dence regardless the operator and environment dynamics:
Criterion Transparency based on kinematic correspondence
1. ql (t) = qr (t).
2. τ h (t) = τ e (t).
3. Both ql (t) = qr (t) and τ h (t) = τ e (t) hold.
Extending the concepts of telepresence and specially that of transparency for sys-
tems with communications delays is not a direct task and there is actually no general
consensus regarding this particular issue. However, in this chapter an attempt to
introduce an objective criterion based on the concept of delayed kinematic corre-
spondence and on the second criterion for systems without time delays is made. In
fact, it would be straightforward to define:
1. qi (t) = q j (t − T j (t)).
2. τ i (t) = τ j (t − T j (t)).
3. Both qi (t) = q j (t − T j (t)) and τ j (t) = τ i (t − Ti (t)) hold.
Certainly, if i = l, then j = r and vice versa. Also, τ l = τ h and τ r = τ e . Both
notations are used throughout the chapter and this should not be a source of confusion.
The previous criterion makes evident that it is not possible in general to get ql (t) =
qr (t − Tr (t)) and vice versa, i.e. qr (t) = ql (t − Tl (t)), simultaneously. Indeed, let’s
choose the desired trajectory for a robot as the delayed one of the other, i.e.
qdi (t) = q j (t − T j (t)), (9.3)
and define as usual the tracking error as
ei = qi − qdi . (9.4)
Assume now that, somehow, it is possible to get ei ≡ 0 in a finite time. Then, accord-
ing to (9.3) and (9.4) one must have
qi (t) = q j (t − T j (t)) = qi (t − T j (t) − Ti (t)). (9.5)
This relationship must be valid despite any human torque τ h and any environmental
force τ e , which allows only two possibilities: either qi (t) = q j (t) = qc for some
constant vector, perhaps satisfying a constraint on the remote side, or a synchronized
movement arises when the sum of the time delays is constant. Therefore, it turns out
that with this particular tracking errors definitions, making them exactly zero prevents
the human operator to cause any desired motion on the local side other than constant
regulation. The same can be said for forces. Therefore, one of the two possibilities
should be chosen in advanced. When the remote manipulator is in free motion it
makes just sense to choose i = r and j = l, but if it is in constrained motion then it
makes more sense to choose i = l and j = r. The reason is that for the first case the
corresponding choice implies that when the remote manipulator is in free motion it
should track the (delayed) trajectory commanded by the human operator at the local
side. In contrast, for the second situation the corresponding choice implies that the
remote surface is being reconstructed at the local side for the human operator. But
regarding the force applied by the remote robot to the environment, it is important
that this be commanded by the human operator, which implies that one should choose
i = r and j = l. This choice appears to be the best because even if the operator does
not know when the remote robot will apply the desired force, he/she knows it simply
because he/she is commanding it. This can be summarized as:
Criterion
Delayed kinematic correspondence
1. When τ h (t) = τ e (t) = 0, it should be achieved that ql (t) = qr (t − Tr (t)) and
qr (t) = ql (t − Tl (t)), meaning that either a consensus arises reaching a constant
equal position or a synchronized periodic movement takes place. This corre-
sponds to the free motion case.
2. For free movement of the remote manipulator, i.e. τ h (t) = 0 but τ e (t) = 0, then
it should be achieved qr (t) = ql (t − Tl (t)), meaning that the remote manipulator
should track the delayed position of the local one.
3. For constrained motion, i.e. τ h (t) = 0 and τ e (t) = 0, then one of the three ideal
responses should be achieved:
a. ql (t) = qr (t − Tr (t))
b. Fe (t) = Fh (t − Tl (t))
c. Both ql (t) = qr (t − Tl (t)) and Fe (t) = Fh (t − Tl (t)) hold,
where relationship (3.64) has been used, i.e.
τ i = J iT (qi )Fi , (9.6)
with J i (qi ) ∈ Rn×n the robot’s geometric Jacobian.

9.2 Control and Observer Design
Note that the term transparency is excluded in the previous criterion on purpose, but
when Tl (t) = Tr (t) ≡ 0 then Criterion 9.1 is recovered. With this in mind the design
of a control-observer scheme for a system like that depicted in Fig. 9.1 is introduced.
First of all, note that under definition (9.3) the derivative of the desired trajectory is
given by

q̇di (t) = 1 − Ṫ j (t) q̇ j (t − T j (t)). (9.7)
9.2 Control and Observer Design 261
Unless programmed, it is not possible to known Ṫ j (t). Assume also that no velocities
measurements are available, so that q̇ j is not known for j = r, l. For that reason an
observer is necessary to obtain an estimate q̂˙ j ; but even with that estimate q̇di (t)
would not be available, so that it can be used
q̇ˆ di = q̂˙ j (t − T j (t)) (9.8)
as a substitution of the not available q̇di , where as usual if i = l, then j = r and vice
versa. Based on the control-observer scheme of Section 6.3, the following velocity
observer is proposed
ξ̇ i = zi (9.9)
ζ i = q̇ˆ di − xi ei + K di zi ξ i (9.10)
q̂˙ = ζ + zi zi + K di zi ,
i i (9.11)
where zi , xi , K di ∈ Rn×n are positive definite diagonal matrices, with the usual
observer error
zi = qi − q̂i , (9.12)
where q̂i is obtained by computing the integral of q̂˙ i in (9.11). The associated control
scheme requires of the following definitions
si = q̂˙ i − q̇ˆ di + xi ei (9.13)

d
σ i = K βi si + sign(si ) (9.14)
dt
q̇oi = q̂˙ i − zi zi (9.15)
q̇ri = q̇ˆ di − xi ei − K γ i σ i (9.16)
soi = q̇oi − q̇ri , (9.17)
where K βi , K γ i ∈ Rn×n are positive definite diagonal matrices and sign(si ) =

[sign(si1 ), . . . , sign(sin )]T with si j element of si for j = 1, . . . , n. When the remote
robot is in free motion, according to (6.82) the input torques should be given by
τ l = −K al q̂˙ l − K pl sol (9.18)

τ r = −K ar q̂˙ − K pr sor
r (9.19)
respectively, where K al , K ar , K pr , K f l ∈ Rn×n are positive definite diagonal matrices.

Note that the main difference with (6.82) is that some damping is added. The reason
is that when the operator drops the local end-effector then the robots should stop its
movement. On the other hand, if the remote robot gets in touch with a surface then it
is more advisable to include some force feedback so that a force tracking error can
be defined as
Fi = Fi − Fdi ≡ Fi − F j (t − T j (t)). (9.20)
A word of caution has to be made. Usually, in practical implementations the human

operator pulls the local end-effector while the remote robot pushes on the environ-
ment. This way, the force sign in the measurements may be the opposite and this
has to be taken into account if necessary. The corresponding integral represents the
linear and angular momenta errors as
t
pi = Fi dt, (9.21)
0
with Fe or Fr for i = r and Fh or Fl for i = l employed indistinctly, and as usual

if i = l then j = r and vice versa. Instead of (9.18)–(9.19), consider the following
control laws
τ l = −K al q̂˙ l − K pl sol + J Tl (ql )(Fdh − K f l pl ) (9.22)

τ r = −K ar q̂˙ − K pr sor − J T (q )(Fde − K fr p ),
r r r r (9.23)
where K fi ∈ Rn×n are positive definite diagonal matrices. One of the main character-
istics of the previous approach is its simplicity. In fact, as made clear by Eq. (6.84), at
heart there is a PID controller for each robot for position tracking, and from (9.22)–
(9.23) it is also clear that one has a PI controller for force tracking. Another key
characteristic is that they are the same both for the local and the remote manipula-
tor. This is one of the reasons why this notation is preferred over the more usual of
master-slave, because any of the robots can be moved by a human operator while
the other will try to follow the command given on the other side. Therefore, even if
no force feedback is available, the human operator will have some sense of telepres-
ence, so that at least Criterion 9.3.1 and Criterion 9.3.3a can be achieved. To show
this, assume that that is precisely the case and compute the error dynamics of sys-
tem (9.1)–(9.2) in closed loop with observer (9.9)–(9.11) and the control laws (9.18)
and (9.19). Define for i = l, r
sqi = q̇i − q̇ri (9.24)

ri = q̇i − q̇oi = żi + zi zi . (9.25)
Now, consider system (9.1) in closed loop with the control law (9.18), and (9.2) in
closed loop with (9.19). By using (9.17), (9.24), (9.25) and by taking into account
that soi = q̇oi − q̇ri = sqi − ri it can be obtained
H i ṡqi + C i sqi + K vi sqi = K ai żi + K pi ri + yai − gi + τ pi , (9.26)


where τ pl = −τ h , τ pr = τ e , yai = − H i q̈ri + C i q̇ri + Di q̇ri + K ai q̇ri , and K vi =
Di + K ai + K pi . For the observers, from (9.10)–(9.11) one gets
q̂˙ i = q̇ˆ di − xi ei + K di zi ξ i + zi zi + K di zi , (9.27)
and from (9.16)
q̂˙ i = q̇ri + K γ i σ i + K di zi ξ i + zi zi + K di zi . (9.28)
By taking into account (9.24)
sqi = żi + K γ i σ i + K di zi ξ i + zi zi + K di zi . (9.29)
By computing the derivative of this expression and by using (9.9) and (9.25) one gets
the observer closed loop dynamics as
d
ṙi + K di ri = ṡqi − K γ i σi. (9.30)
dt
Define the error state for the closed loop dynamics (9.26) and (9.30) as
⎡ ⎤
sql
⎢ sqr ⎥
x=⎢ ⎥
⎣ rl ⎦ . (9.31)
rr
Furthermore, define as usual a region of interest of the form (4.28)
D = x ∈ R4n |x < ,
for some 0 < < ∞. It can be shown in the very same fashion as done in Section 6.3
that whenever x is bounded by (i.e. x ∈ D), then any other signal related to the
closed loop system dynamics is bounded, as long as the following assumption is
made.
Assumption 9.1 The human and environment torques as well as their derivatives are
bounded for all time, i.e. there exist positive constants bh , bdh , be and bde such that
Fh ≤ bh < ∞, dtd Fh ≤ bdh < ∞, Fe ≤ be < ∞, and dtd Fe ≤ bde < ∞ ∀
t ≥ t0 . Furthermore, the robot manipulators do not reach any singularity so that the
inverse matrices J i−1 (qi ) always exist.
With this in mind, the following procedure is completely similar to that given in
Section 6.3:
1. The first step is to show how to set gains to make x(t) bounded for all time, i.e.
x ∈ D ∀ t ≥ t0 , as long as the initial condition x(t0 ) is also contained in D. In
order to do this, consider
1 T
V (x) = x Mx, (9.32)
2
with M = block diag {H l (ql ), H r (qr ), I, I}, which satisfies (4.29) after Prop-
erty 3.7, i.e.
α1 (x) = λ1 x2 ≤ V (x) ≤ α2 (x) = λ2 x2 , (9.33)
with λ1 = 21 λmin (M) and λ2 = 21 λmax (M). By taking into account Property 3.2,
the derivative of V (x) along (9.26) and (9.30) is given by

V̇ (x) = − sTql K vl sql + sTql K al żl + K pl rl + yal − gl − τ h

− sTqr K vr sqr + sTqr K ar żr + K pr rr + yar − gr + τ e

d
− rl K dl rl + rl ṡql − K γ l σ l
T T
dt

d
− rr K dr rr + rr ṡqr − K γ r σ r
T T
dt
≤ − λmin (K vl )sql 2 − λmin (K vr )sqr 2

+ sql λmax (K al )żl + λmax (K pl )rl
+ sql yal − gl − τ h

+ sqr λmax (K ar )żr + λmax (K pr )rr
+ sql yar − gr + τ e

d
− λmin (K dl )rl 2 + rl ṡ
ql − K σ
dt
γl l

d
− λmin (K dr )rr 2 + rr ṡ
qr − K σ
r . (9.34)
dt
γr
Since the stability analysis is valid only in D, there must exist positive constants
such that
αl = max yal (9.35)

∀x∈D
αr = max yar (9.36)
∀x∈D

d
al = max ṡ − K σ (9.37)
∀x∈D dt
ql γl l

d
ar = max ṡ − K σ
r . (9.38)
∀x∈D dt
qr γr
Also, in D it is satisfied
zi ≤ z max i , (9.39)

for some positive constant z max i , while directly from (9.25) one gets
żi ≤ λmax (zi )z imax + ri . (9.40)
Then, by taking into account (9.35)–(9.40), Property 3.11 and Assumption 9.1,
it is possible to rewrite V̇ (x) in (9.34) as
V̇ (x) ≤ − λmin (K vl )sql 2 + βl sql rl − λmin (K dl )rl 2

− λmin (K vr )sqr 2 + βr sqr rr − λmin (K dr )rr 2
+ βx, (9.41)
where βl , βr and β are defined as

βl = λmax (K al ) + λmax (K pl ) (9.42)

βr = λmax (K ar ) + λmax (K pr ) (9.43)
β =αl + αr + al + ar + kgl + kgr + bh + be
+ λmax (K al )λmax (zl )z max l + λmax (K ar )λmax (zr )z max r . (9.44)
Suppose gains are chosen to satisfy
λmin (K vl ) ≥ 1 + 2δ (9.45)
λmin (K vr ) ≥ 1 + 2δ (9.46)
βl2
λmin (K dl ) ≥ + 2δ (9.47)
4
β2
λmin (K dr ) ≥ r + 2δ, (9.48)
4
where δ is a positive constant. Then, it is straightforward to get
V̇ (x) ≤ −2δx2 + βx = −δx2 − x(δx − β). (9.49)
Define
β
μ= . (9.50)
δ
Then, whenever x ≥ μ one has
V̇ (x) ≤ −δx2 = −W3 (x). (9.51)
By taking into account (9.33), conditions (4.29) and (4.53) of Theorem 4.5 are
fulfilled in D. According to the same theorem, the initial condition must comply
with (4.56), i.e.

λ1
x(t0 ) ≤ ,
λ2
and after (4.73), the final state will be ultimately bounded by

λ2
x(t) ≤ μ = bf . (9.52)
λ1

Note that one must enforce μ < λλ21 by setting gains large enough, but it is
not necessary to make the ultimate bound bf arbitrarily small. This proves that
all tracking and observation errors remain bounded for all time.
2. Once it has been shown that x is bounded (and thus every single error), the next
step is to show that the observation errors tend to zero. Consider (9.16) and (9.24)
to get sqi = q̇i − q̇ˆ di + xi ei + K γ i σ i , and after (9.13)
si + K γ i σ i = sqi − żi . (9.53)
By computing its derivative, it is after (9.14) and (9.53)

ṡi = −K γ i K βi si + sign(si ) + ṡqi − z̈i . (9.54)
By using this equation and the fact that ṡqi − z̈i is bounded in D, it can be
shown that si ≡ 0 in a finite time tr ≥ 0 as shown in Section 6.3. Then, take into
account (9.9)–(9.13) to get
si = (zi + K di ) zi + K di zi ξ i . (9.55)
Once si ≡ 0 one can compute
(zi + K di ) żi + K di zi zi = 0, (9.56)
which represents a stable linear filter for zi with input zero, so that zi , żi → 0.
3. While the observations errors tend to zero, the tracking errors can only be shown
to be bounded in a finite time. To prove this, recall that si = 0 in a finite time
tr ≥ t0 , so that from (9.8) and (9.13) one has
q̇i − q̇ j (t − T j (t)) + xi ei = żi − ż j (t − T j (t)). (9.57)
Consider now that from (9.7) one has
ėi + xi ei = żi − ż j (t − T j (t)) + Ṫ j (t)q̇ j (t − T j (t)), (9.58)

which is a stable linear filter of the form
ėi = −xi ei + ui , (9.59)
with
ui = żi − ż j (t − T j (t)) + Ṫ j (t)q̇ j (t − T j (t)). (9.60)
At this point it is necessary to make the following assumption.

Assumption 9.2 The time delays derivatives are bounded by Ti∗ , i.e. 0 ≤ |Ṫi (t)| ≤
Ti∗ < ∞, for i = l, r.
Note that Assumption 9.2 requires the time delays derivatives to be bounded, but
not the delays themselves. This implies that ui must be bounded for all t ≥ t0 since
it has been shown that x ∈ D for all t ≥ t0 . Since system (9.59) is time invariant it
must hold
e(t−t0 )xi ≤ ki e−λi (t−t0 ) , (9.61)
and the state, i.e. ei , is bounded by
ki
ei ≤ ki e−λi (t−t0 ) ei (t0 ) + sup ui (ϑ), (9.62)
λi 0≤ϑ≤t
for some positive constants ki and λi . Clearly, the first term will vanish as t → ∞,
but it is necessary to show that λkii can be made arbitrarily small. To simplify the
discussion, suppose that the eigenvalues of xi are chosen all real and distinct,
satisfying
λin < λi(n−1) < · · · < λi1 < 0. (9.63)
Then, the right eigenvectors d i j with j = 1, . . . , n satisfy
xi d i j = λi j d i j , (9.64)
while for the corresponding left eigenvector ηi j with j = 1, . . . , n it holds
ηiTj xi = λi j ηiTj . (9.65)
Note that it is assumed that d i j = ηi j = 1 ∀ i, j = 1, . . . , n and that it holds
ηik
T
d il = δkl , (9.66)
for k, l = 1, . . . , n, where δkl is the Kronecker symbol satisfying δkl = 0 if k = l

and δkk = 1. Under these conditions, it can be shown that

n
e(t−t0 )xi = eλi j (t−t0 ) d i j ηiTj . (9.67)
j=1
By taking norms one has

n
n
e(t−t0 )xi ≤ eλi j (t−t0 ) d i j ηi j = eλi j (t−t0 ) , (9.68)
j=1 j=1
and from (9.63) it is

n
e(t−t0 )xi ≤ eλi1 (t−t0 ) = neλi1 (t−t0 ) = ne−λi (t−t0 ) , (9.69)
j=1
with λi = |λi1 |. By setting ki = n one gets (9.61). Recalling that every variable is
bounded in D and taking into account Assumption 9.2, there must exist a positive
constant u max i so that ui ≤ u max i < ∞ ∀ t ≥ t0 . Since e−λi t → 0, then for a large
enough time the ultimate bound for ei will satisfy according to (9.62)
ki
ei (t) ≤ u max i = δmax i . (9.70)
λi
Since λi = min |λ(xi )|, by choosing λi large enough then δmax i becomes arbitrarily
small for i =l, r.
4. Suppose that τ h = τ e = 0, then all tracking errors tend to zero and consensus is
achieved, i.e. ql (t) = qr (t) = qc for some constant qc . This behavior corresponds
to Criterion 9.3.1 and to prove it assume that the bounds δmax i in (9.70) are chosen
small enough to make the tracking error negligible, i.e. el ≈ 0 and er ≈ 0, so
that according to (9.3) and (9.4) the following holds
qi (t) ≈ q j (t − T j (t)) ≈ qi (t − Ti (t) − T j (t)), (9.71)
which means that
qi (t) ≈ qi (t − Ti (t) − T j (t)). (9.72)
Equation (9.72) is valid for any unrelated variable time delays Ti (t) and T j (t) and
for an arbitrarily small tracking error, which in general can only be satisfied if and
only if qi (t) ≡ qc for some constant vector qc . This also implies that q̇i (t) → 0 so
that the input of the filter in (9.58) becomes zero even if time delays are variable,
meaning that one has ėi , ei → 0 exactly, not only approximately. For that reason
one must have qr (t) = ql (t) ≡ qc , i.e. consensus is achieved. Of course, it is
worthy to mention that the robots can also have a synchronized movement in
the sense that they track each other with a periodic behavior if the round delay
is constant, i.e. the sum Ti (t) + T j (t) is constant. The reason is that (9.72) can
also be fulfilled if the manipulators track each other with a periodic behavior.
This, however, depends on the particular characteristics of the time delays and
it is a behavior that cannot be induced by gain tuning. Note that consensus is
guaranteed for τ h = τ e = 0 without needing the exact knowledge of the gravity
vector, which represents an advantage over previous approaches.
5. Suppose that τ h = 0 but τ e = 0 but one still has el ≈ 0. For that case the
human operator will not be able to move the local manipulator and the previous
item 4 will still be valid because (9.70) holds regardless τ h and τ e , i.e. regardless
the values of bh and be , although according to (9.45)–(9.48), δ has to be chosen
larger to keep the same value of μ in (9.50) as for the case when τ h = τ e = 0.
Thus, el ≈ 0 means that the human operator will be treated as a perturbation
and rejected by the controller, so that he/she will not be able to move the local end-
effector according with (9.72). In order to avoid this undesirable effect el ≈ 0
should not hold so that the human operator be able to move the local end-effector,
while the remote tracking error er can as before be made arbitrarily small. This
corresponds to Criterion 9.3.2.
6. Suppose that τ h = 0 and τ e = 0 and assume that el ≈ 0 does not hold, so
that the human operator is able to move the local end-effector. Then, once the
movement of the remote robot is constrained, he/she will undergo delayed kine-
matic correspondence in the sense that he/she will no longer be able to freely
move the local end-effector in the direction the remote manipulator is applying
the contact force and the remote contact surface will locally be reproduced with
an error no larger than δmax l . This is true because the value of el cannot be
larger than δmaxl in (9.70), meaning that the remote surface is being reproduced
with an error no larger than δmax l . This corresponds to Criterion 9.3.3a.
7. Item 6 makes clear that force feedback is mandatory to achieve Criterion 9.3.3c.
To show this, under the control laws (9.22)–(9.23), the closed loop dynamics is
once again given by (9.26), but this time with
τ pi = ∓J iT (qi )(Fi + K fi pi ), (9.73)
where the − sign is for i = l and the + sign is for i = r. Note that (9.6) has
been used to get (9.73). Thus, should τ pl and τ pr be bounded, then exactly the
very same conclusions can be drawn as before, i.e. items 1–6 hold. However,
while Assumption 9.1 guarantees the boundedness of Fi , that of pi cannot
be assumed and it has to be enforced by gain tuning. To do so, it is convenient
to rewrite (9.26) as
H i ṡpi + C i spi + K vi spi = K ai żi + K pi ri + ybi − gi , (9.74)
where

spi = sqi − K −1 −1 T
vi τ pi = sqi ± K vi J i (qi ) Fi + K fi pi , (9.75)
and

ybi = − H i (q̈ri + K −1 −1
vi τ̇ pi ) + C i (q̇ri + K vi τ pi ) + Di q̇ri + K ai q̇ri .
(9.76)
Now, change the definition given in (9.31) by

⎡ ⎤
spl
⎢ spr ⎥
x=⎢ ⎥
⎣ rl ⎦ . (9.77)
rr
Clearly, the positive definite function V (x) in (9.32) can be employed once
again, with a proper new region D. Note that new but fully equivalent equations
of the form (9.33) to (9.52) can be gotten as long as pi is bounded because
d
dt
Fi is bounded by Assumptions 9.1 and 9.2. Certainly, the values of the
constants in (9.35)–(9.38) and (9.42)–(9.44) would be different, but that remains
a minor detail since ultimate boundedness of the system trajectories could still be
guaranteed by gain tuning just as before. Therefore, only the boundedness of pi
needs to be proven. Recall that by Assumption 9.1 the inverse J i−T (qi ) always
exists. Then, to ensure the boundedness of pi , eq. (9.75) can be rewritten as

Fi = −K fi pi ∓ J i−T (qi )K vi spi − sqi (9.78)
to allow to describe the system’s closed loop dynamics by (9.30), (9.74) and
(9.78) and to define a new closed loop state as
⎡ ⎤
spl
⎢ spr ⎥
⎢ ⎥
⎢ rl ⎥ x
⎢
y=⎢ ⎥
⎥ = p . (9.79)
⎢ rr ⎥
⎣ pl ⎦
pr
As done before for the original state x, a region of interest
Dy = y ∈ R6n |y < ,
for some 0 < < ∞ can be defined for the state y, so that the boundedness of
every signal of interest is guaranteed in Dy as before. The usefulness of (9.30),
(9.74) and (9.78) is that being pi part of the state, then conditions can be found
to enforce the boundedness of y by using Theorem 4.5. The main goal is to
obtain an equation of the form (9.49) but for the state y. As a matter of fact,
as said before and without any lost of generality V (x) in (9.32) can be used to
arrive exactly to (9.49) (in terms of the new x) just by properly changing the
constants definitions in (9.35)–(9.38) and (9.42)–(9.44) for the new region Dy

and of course making the corresponding change of notation for the elements
of x defined in (9.77). For simplicity’s sake the same notation of the different
constants β is kept. On the other hand, define
1 T 1 1
Vp (p) = p p = pTl pl + pTr pr , (9.80)
2 2 2
and
1 T
Vy (y) = V (x) + Vp (p) = y Ny, (9.81)
2
where N = block diag {H l (ql ), H r (qr ), I, I, I, I}. As before, it satisfies condi-
tion (4.29) after Property 3.7 with α1 (y) = λy1 y2 , and α2 (y) = λy2 y2
for λy1 = 21 λmin (N) and λy2 = 21 λmax (N). Then, by direct computation it can be
shown that V̇y (y) ≤ −δy2 holds for some δ > 0 by setting gains large enough
whenever y ≥ μ with μ arbitrary small. This allows to employ once again
Theorem 4.5. Certainly, this proves that it is always possible to find a set of
gains to keep p bounded. As previously discussed, just the boundedness of
p ensures that items 1–6 still hold. From (9.78) it is possible to obtain
d
Fi = −K fi Fi ∓ ωi (9.82)
dt
with ωi = dt d J −T (q )K s − s bounded in D . Since (9.82) represents a

i i vi pi qi y
stable linear filter, large values of K fi imply that the ultimate bound of Fi can
be made arbitrarily small. However, as explained before for position tracking,
due to the time delays this leads to a contradiction unless the human and contact
forces tend to be constant and equal. In particular, an equation of the form (9.72)
would arise but for human and environment forces. On the contrary, setting only
K fr large enough will allow the remote manipulator to track the delayed force
applied by the operator, which corresponds to Criterion 9.3.3c.
In this section, an experimental evaluation of the scheme developed in this chapter

is presented. The constant and time-varying delay cases are considered. The experi-
mental setup consists of two identical Geomagic Touch manipulators, equipped each
one with an ATI Nano17 force sensor. The three-degree-of-freedom configuration
described in Section 14.2.5 is employed, i.e. the last three joints are mechanically
blocked.
Tuning guide
The equations required to implement the bilateral teleoperation scheme presented in
this chapter are the tracking error definitions (9.3) and (9.4), the relationship (9.6),
the observer (9.9)–(9.12, the auxiliary variables (9.13)–(9.17), the force error def-
initions (9.20) and (9.21), and the control laws (9.22) and (9.23). The following
suggestions can be followed to tune the controller.
K fi = K fi = K βi = K γ i = O, i = l, s.
• Tune the remaining position tracking controllers as standard PD ones, with K pi as
the derivative gain matrices and xi = K −1 pi K pxi , where K pxi are the proportional
gain matrices.
• Add damping by setting K al and K ar to obtain a stable teleoperation in presence
of time-delays. Note that the larger these gains, the more stable will be the teleop-
erator, but at the expense of making it stiffer.
• Increase the gains K γ i slowly to improve the tracking performance, but preventing
the actuators to present chattering.
• Increase the gains K βi slowly in the same manner.
• Include the force control terms and increase K fl and K fr until obtaining the desired
Experiments
The gains employed to evaluate the scheme of this chapter for both constant and
time-varying delays, obtained by following the suggestions above are summarized
in Table 9.1.
Constant time-delay
For this scenario, asymmetrical delays are artificially created for each robot, being
Tl = 0.5 s for the local manipulator and Tr = 0.4 s for the remote one. The experiment
starts in free motion for about 40 s, then enters in contact with a rigid surface, and
finally returns to free motion again.
Table 9.1 Chosen gains for the bilateral teleoperation controller

Gain Value
K pl , K pr 0.02I
xl , xr 10I
K dl , K dr 100
zl , zr 10I
K βl , K βr 0.5I
Kγ l, Kγ r 4I
K al , K ar 0.02I
K fl , K fr 0.001I
Fig. 9.2 Constant time-delay, joint position tracking: ql (t − Tr (t)) (—), qr (– –)
The tracking position in joint coordinates for this case is shown in Fig. 9.2, whereas
the corresponding tracking error is displayed in Fig. 9.3. A comparison between the
joint velocities obtained through a numerical derivative and the ones estimated by
the observer is presented in Fig. 9.4 for the local, and in Fig. 9.5 for the remote
manipulator. The contact force along with the force reflected to the local operator is
shown in Fig. 9.6, with their corresponding force tracking errors displayed in Fig. 9.7.
Time-varying delay
In this case, again, asymmetrical delays are artificially created for each robot, but
time-varying. The time-delay for the local manipulator is created through a ran-
dom function taking values in the interval Tl ∈ [0.45, 0.55] s, whereas the time-
delay random function for the remote manipulator takes values in the interval
Tr ∈ [0.65, 0.75] s. A plot of the mentioned time-varying delays is shown in Fig. 9.8.
The experiment starts in free motion for about 30 s, then enters in contact with a rigid
surface, and finally returns to free motion again.
Fig. 9.3 Constant time-delay, joint position tracking error
The tracking position in joint coordinates for this case is shown in Fig. 9.9, with
their corresponding tracking errors displayed in Fig. 9.10. A comparison between
the joint velocities obtained through a numerical derivative and the ones estimated
by the observer is presented in Fig. 9.11 for the local, and in Fig. 9.12 for the remote
manipulator. The contact force along with the force reflected to the local operator
is shown in Fig. 9.13, with their corresponding force tracking errors displayed in
Fig. 9.14.
When comparing the graphics of the constant delay case with those of the time-
varying delay, it can be seen that even the relatively low time-variation of 0.1 s,
affects in a great manner the performance of the position tracking, particularly when
interacting with the environment. Nevertheless, it can be also appreciated that the
teleoperator remained stable during all the experiments.
Fig. 9.4 Constant time-delay, joint velocity estimation for the local manipulator: observer (—),
numerical (– –)
Fig. 9.5 Constant time-delay, joint velocity estimation for the remote manipulator: observer (—),
numerical (– –)
Fig. 9.6 Constant time-delay, Force tracking: Fl (t − Tr (t)) (—), Fr (– –)
Fig. 9.7 Constant time-delay, Force tracking error

Fig. 9.8 Variable time-delay, time delays: Tl (t) (up), Tr (t) (down)
Fig. 9.9 Variable time-delay, joint position tracking: ql (t − Tr (t)) (—), qr (– –)

Fig. 9.10 Variable time-delay, joint position tracking error.
Fig. 9.11 Variable time-delay, joint velocity estimation for the local manipulator: observer (—),
numerical (– –)
Fig. 9.12 Variable time-delay, joint velocity estimation for the remote manipulator: observer (—),
numerical (– –)
Fig. 9.13 Variable time-delay, Force tracking: Fl (t − Tr (t)) (—), Fr (– –)

Fig. 9.14 Variable time-delay, Force tracking error
References
Aldana CI, Romero E, Nuño E, Basañez L (2015) Pose consensus in networks of heterogeneous
robots with variable time delays. Int J Robust Nonlinear Control 25(14):2279–2298
Arteaga-Pérez MA, López M, Nuño E, Hernández-Ortiz O (2019) On the delayed kinematic cor-
respondence with variable time delays for the control of the bilateral teleoperation of robots. Int
J Control. https://doi.org/10.1080/00207179.2019.1707287
Arteaga-Pérez MA, Morales M, López M, Nuño E (2018) Observer design for the synchronization
of bilateral delayed teleoperators. Eur J Control 43:20–32. https://doi.org/10.1016/j.ejcon.2018.
06.001
Callier FM, Desoer CA (1991) Linear system theory. Springer, New York
Chopra N, Spong MW, Lozano R (2008) Synchronization of bilateral teleoperators with time delay.
Automatica 44:2142–2148
Hashtrudi-Zaad K, Salcudean SE (2002) Transparency in time-delayed systems and the effect of
local force feedback for transparent teleoperation. IEEE Trans Robot Autom 18(1):108–114
Khalil HK (2002) Nonlinear systems, 3rd edn. Prentice-Hall, Upper Saddle River
Lawrence DA (1993) Stability and transparency in bilateral teleoperation. IEEE Trans Robot Autom
9(5):624–637
References 281
Nuño E, Basañez L, Ortega R (2011) Passivity-based control for bilateral teleoperation: a tutorial.
Automatica 47(3):485–495
Passenberg C, Peer A, Buss M (2010) A survey of environment-, operator-, and task-adapted con-
trollers for teleoperation systems. Mechatronics 20:787–801
Slawiñski E, Mut VA, Fiorini P, Salinas LR (2012) Quantitative absolute transparency for bilateral
teleoperation of mobile robots. IEEE Trans Syst Man Cybern Part A Syst Humans 42(2):430–442
Yokokohji Y, Yoshikawa T (1994) Bilateral control of master-slave manipulators for ideal kinesthetic
coupling: formulation and experiments. IEEE Trans Robot Autom 10(5):605–620
Chapter 10
Robot Networks
Abstract The consensus problem for networks of multiple agents consists in reach-
ing an agreement between certain coordinates of interest using a distributed con-
troller. For a robot network, there are multiple communication channels, each of
them possibly with a time varying delay, which makes the stability analysis and the
control design more complex. A common way to analyze the consensus problem
for this case is by employing algebraic graphs. Usually, the graph is assumed to be
undirected and connected. This chapter deals with the design of a control law with a
velocity observer to solve the Leader Follower Consensus Problem and the Leader-
less Consensus Problem in networks of fully actuated robots modeled as undirected
weighted graphs.
10.1 Basic Characteristic of Robot Networks
When three or more robot manipulators are interconnected instead of a bilateral

system one has a robot network like that depicted in Fig. 10.1. The robots may or
may not have the same kinematic configuration, but for simplicity’s sake it is assumed
that all the manipulators of the network are similar. Just as for bilateral control, one of
the main problems to be solved is that of consensus. For instance, it may be desirable
that all the agents find a consensus at a given leader coordinates (Leader Follower
Consensus Problem LFCP), or it may be only necessary that the agents agree at a
certain coordinates value (Leaderless Consensus Problem LCP). In any case, to find
a solution it is first necessary to establish some of the characteristics of the network.
The interconnection of N manipulators can be modeled using graph theory. Note
that a deep description of graph theory is out of the scope of this book and therefore
strictly only the concepts necessary to solve the LFCP and the LCP are given. First
of all each manipulator of the network is considered a node of the graph, i.e. there
are N nodes. The ordered pairs of nodes are called edges. For an undirected graph
the edge (i, j) denotes that agents i and j can obtain information from each other,
roughly meaning that when two manipulators of the network have communication,
then this is bilateral (in contrast to a directed graph). Besides the assumption that
the graph is undirected, it is assumed that it is weighted, meaning that a weight is
https://doi.org/10.1007/978-3-030-85980-0_10
284 10 Robot Networks
Fig. 10.1 Components of a bilateral teleoperation system
associated to every edge in the graph. An undirected path is a sequence of edges of

the form (i 1 , i 2 ), (i 2 , i 3 ), . . ., while the undirected graph is said to be connected if
there is an undirected path between every pair of distinct nodes.
Assumption 10.1 The network graph is undirected and connected.
Let n i denote the number of robots transmitting information to the i-th manipulator
and Ni the set containing them. This can be described by N × N constants ai j
for i, j = 1, . . . , N , so that ai j > 0 if j ∈ Ni and ai j = 0 otherwise. Furthermore,
aii = 0. Then, the network interconnection can be described by using the so-called
Laplacian matrix L ∈ R N ×N , whose elements are defined by
⎧
⎪ aik i = j
⎪ N
⎨
li j = k=1 (10.1)
⎪
⎪
⎩
−ai j i = j
Assumption 10.1 ensures that the Laplacian matrix L is symmetric because ai j =

a ji ⇒ li j = l ji . Also, it can be shown that all its eigenvalues are positive but one single
zero eigenvalue, i.e. L is positive semidefinite. In fact, by the definition given in (10.1)
any eigenvector associated to that single zero eigenvalue is given by α1 N ∈ R N with
α any real value different from zero and 1 N a column vector filled with ones.
Finally, the dynamics of the ith manipulator is given by (3.44), i.e.
H i (qi )q̈i + C i (qi , q̇i )q̇i + Di q̇i + gi (qi ) = τ i , (10.2)
for i = 1, . . . , N .
10.2 Leaderless Consensus Problem (LCP) 285
10.2 Leaderless Consensus Problem (LCP)
For the leaderless consensus problem (LCP) the control objective is that all robot
positions reach a consensus with zero velocity, i.e. for all i = 1, . . . , N there exists
qc ∈ Rn such that
lim qi (t) = qc and lim q̇i (t) = 0. (10.3)

t→∞ t→∞
One of the most common approaches to achieve this goal is to use a PD control law
with damping inclusion and gravity cancellation of the form

N
τ i = gi (qi ) − pi ai j (qi − q̄ j ) − di q̇i (10.4)
j=1
q̄ j = q j (t − T ji (t)), (10.5)
where pi , di ∈ R are positive gains, for i, j = 1, . . . , N . However, if velocity mea-

surements are not available a conceptually similar observer to that given in Section 6.2
is introduced here as
q̂˙ i = zi zi + K di zi + ξ i (10.6)

N
ξ̇ i = K di zi zi − pi H i−1 (qi ) ai j (qi − q̄ j ), (10.7)
j=1
where zi , K di ∈ Rn×n are positive definite and diagonal matrices and (ˆ·) is the
estimated value of (·). The usual observation error definition (6.1)
zi = qi − q̂i ,
is employed. Note that the observer (10.6)–(10.7) requires the knowledge of the N
inertia matrices, while the control law (10.4) becomes

N
τ i = gi (qi ) − pi ai j (qi − q̄ j ) − di q̂˙ i . (10.8)
j=1
To compute the closed loop dynamics, from (10.2), (10.8) and (6.1) it is

N
H i (qi )q̈i + C i (qi , q̇i )q̇i + K vi q̇i + pi ai j (qi − q̄ j ) − di żi = 0, (10.9)
j=1
where
K vi = Di + di I. (10.10)
As to the observer closed loop dynamics, once again a sliding variable of the
form (6.45) is employed, i.e.
ri = żi + zi zi . (10.11)
Then, combine (10.6)–(10.7) to get

N
q̂¨ i = zi żi + K di żi + K di zi zi − pi H i−1 (qi ) ai j (qi − q̄ j ), (10.12)
j=1
and use ṙi = q̈i − q̂¨ i + zi żi to have

N
q̈i = ṙi + K di ri − pi H i−1 (qi ) ai j (qi − q̄ j ), (10.13)
j=1
Finally, use (10.9) to get

ṙi = −K di ri − H i−1 (qi ) C i (qi , q̇i )q̇i + K vi q̇i − di żi . (10.14)
To show that the control objective (10.3) can be achieved with this control-observer
scheme define the error
ei j = qi − q j , (10.15)
for i, j = 1, . . . , N . Recall that ai j > 0 if and only if j ∈ N , so that ei j doesn’t exist

if j ∈
/ Ni .
As it is the main proposal of this book, local stability is pursued, for which it is
necessary to define the following error state
⎡ ⎤
ei1
⎢ .. ⎥
⎢ . ⎥
⎢ ⎥
⎢ ⎥
xi = ⎢ ei N ⎥ , (10.16)
⎢ q̇ ⎥
⎢ i ⎥
⎣ ri ⎦
zi
for each subsystem dynamics (10.9), (10.11) and (10.14) and where xi ∈ Rn(3+ni ) .
Then consider also for each manipulator the following positive definite function
1
N
1 T 1 1
Vi = q̇i H i (qi )q̇i + ai j qi − q j 2 + riT ri + γi ziT −1
zi zi , (10.17)
2 pi 4 j=1 2 2
with γi > 0. As usual, by taking Property 3.7 into account, condition (4.29) holds
for each Vi , i.e.
αi1 (xi ) = λi1 xi 2 ≤ Vi ≤ λi2 xi 2 = αi2 (xi ), (10.18)
with

λhi 1 1 1 γi
λi1 = min , min ai j , , (10.19)
2 pi 4 j∈Ni 2 2 λmax (zi )
and

λHi 1 1 1 γi
λi2 = max , max ai j , , . (10.20)
2 pi 4 j∈Ni 2 2 λmin (zi )
This way, for the whole robot network the following positive definite function can
be used

N
V (x) = Vi , (10.21)
i=1
with
⎡ ⎤
x1
⎢ ⎥
x = ⎣ ... ⎦ . (10.22)
xN
Define a domain of interest D as in (4.28), i.e.

D = x ∈ Rn(3N +n 1 +...+n N ) \x < ,
for any > 0. Now, by setting λ1 = min{λi1 } and λ2 = max{λi2 }, it is possible to

get condition (4.29) for the whole system as well, i.e.
α1 (x) = λ1 x2 ≤ V (x) ≤ λ2 x2 = α2 (x). (10.23)
By using Property 3.2, the derivative of Vi in (10.17) along (10.9), (10.11) and (10.14)
can directly be computed as
⎛ ⎞
1 T⎝ N
V̇i = − q̇i K vi q̇i + pi ai j (qi − q̄ j ) − di żi ⎠
pi j=1
1
N
+ ai j (qi − q j )T (q̇i − q̇ j )
2 j=1

− riT K di ri − riT H i−1 (qi ) C i (qi , q̇i )q̇i + K vi q̇i − di żi + γi ziT −1
zi żi
di di
≤ − q̇i 2 + q̇iT (−zi zi + ri ) − riT K dhi ri
pi pi
N
1
N
− q̇iT ai j (qi − q̄ j ) + ai j (qi − q j )T (q̇i − q̇ j )
j=1
2 j=1

− ri H i (qi ) C i (qi , q̇i )q̇i + K vi q̇i + di zi zi − γi ziT zi + γi ziT −1
T −1
zi ri ,
(10.24)
where
K dhi = K di − di H i−1 (qi ), (10.25)
and (10.10) has been used. Since
t
q j − q̄ j = q j − q j (t − T ji (t)) = q̇ j (θ )dθ, (10.26)
t−T ji (t)
one gets
⎛ ⎞

N
N t
⎜ ⎟
q̇iT ai j (qi − q̄ j ) = q̇iT ai j ⎝qi − q j + q̇ j (θ )dθ ⎠
j=1 j=1 t−T ji (t)

N
N t
= q̇iT ai j (qi − q j ) + q̇iT ai j q̇ j (θ )dθ. (10.27)
j=1 j=1 t−T ji (t)
Then one has
1
N N
ai j (q̇i − q̇ j )T (qi − q j ) − q̇iT ai j (qi − q j ) =
2
j=1 j=1

1
N N
1
ai j (q̇i − q̇ j )T − q̇iT (qi − q j ) = − ai j (q̇i + q̇ j )T (qi − q j ).
2 2
j=1 j=1
(10.28)
Therefore, (10.24) can be rewritten as
di di
V̇i ≤ − q̇ 2 + q̇iT (−zi zi + ri ) − riT K dhi ri
pi i pi
t
1
N N
− T
ai j q̇i q̇ j (θ )dθ − ai j (q̇i + q̇ j )T (qi − q j )
j=1
2 j=1
t−T ji (t)

− riT H i−1 (qi ) C i (qi , q̇i )q̇i + K vi q̇i + di zi zi − γi ziT zi + γi ziT −1
zi ri .
(10.29)
The next step consists in adding all the derivatives V̇i to get the derivative of V (x).
However, prior to do it consider the following

N
N
ai j (q̇i + q̇ j )T (qi − q j ) =
i=1 j=1

N
N
ai j q̇iT qi − q̇iT q j + q̇Tj qi − q̇Tj q j =
i=1 j=1
N

ai1 q̇iT qi − q̇iT q1 + q̇T1 qi − q̇T1 q1 + · · · + ai N q̇iT qi − q̇iT q N + q̇TN qi − q̇TN q N =
i=1

a11 q̇T1 q1 − q̇T1 q1 + q̇T1 q1 − q̇T1 q1 + · · · + a1N q̇T1 q1 − q̇T1 q N + q̇TN q1 − q̇TN q N +
..
.

a N 1 q̇TN q N − q̇TN q1 + q̇T1 q N − q̇T1 q1 + · · · + a N N q̇TN q N − q̇TN q N + q̇TN q N − q̇TN q N = 0.
The last result holds because ai j = a ji and aii = 0 for i, j = 1, . . . , N . Therefore,

the derivative of V (t) in (10.21) satisfies
N
di di
V̇ ≤ − q̇i 2 + q̇iT (−zi zi + ri )
i=1
pi pi

N t
− ai j q̇iT q̇ j (θ )dθ − riT K dhi ri
j=1 t−T ji (t)

−riT H i−1 (qi ) C i (qi , q̇i )q̇i + K vi q̇i + di zi zi − γi ziT zi + γi ziT −1
zi ri . (10.30)
Since the stability analysis is only intended to be valid in D, then from the state
definitions (10.16) and (10.22) it holds
q̇i < . (10.31)
By taking into account K dhi = K di − di H i−1 (qi ), and Properties 3.7 and 3.9 one gets
from (10.30)
N
di di di
V̇ ≤ − q̇i 2 + λmax (zi )q̇i zi + q̇i ri
i=1
pi pi pi

N t
− ai j q̇iT q̇ j (θ )dθ
j=1 t−T ji (t)
di
− λmin (K di )ri 2 + ri 2
λhi
kci + λmax (K vi ) di
+ q̇i ri + λmax (zi )zi ri
λhi λhi

γi
− γi zi 2 + zi ri . (10.32)
λmin (zi )
For simplicity define
di kci + λmax (K vi )
βi = + , (10.33)
pi λhi
to rewrite V̇ as
N
di di
V̇ ≤ − q̇i 2 + βi q̇i ri + λmax (zi )q̇i zi
pi pi
i=1

N t
− ai j q̇iT q̇ j (θ)dθ
j=1 t−T ji (t)
! " ! "
di di γi
− λmin (K di ) − ri 2 + λmax (zi ) + zi ri − γi zi 2 .
λhi λhi λmin (zi )
(10.34)
Choose gains as
di ≥ pi (2 + δ̄di ) (10.35)
! "2
1 di
γi ≥ λmax (zi ) + 1 (10.36)
4 pi
! "2
di 1 di γi β2
λmin (K di ) ≥ + λmax (zi ) + + i + δri (10.37)
λhi 4 λhi λmin (zi ) 4
where δri and δ̄di are positive constants. With this choice it is quite direct to get
⎧ ⎫
N ⎪
⎨
N t ⎪
⎬
V̇ ≤ −δ̄di q̇i 2 − δri ri 2 − ai j q̇iT q̇ j (θ )dθ . (10.38)
⎪ ⎪
i=1 ⎩ j=1 t−T ji (t)
⎭
Despite it was possible to define as usual a region of interest D, it is however not

possible to employ Theorem 4.5 as for some other schemes described in this book.
Instead, it is preferable to use Lemma 4.3, for which it is necessary to make the
following:
Assumption 10.2 The information exchanged from the j-th robot to the i-th one
is subject to a variable time delay T ji (t) with a known upper bound T ji , i.e. 0 ≤
T ji (t) ≤ T ji < ∞. Moreover, dtd T ji (t) is bounded.
By taking into account Assumption 10.2, it is possible to integrate V̇ to get
t
V̇ (σ )dσ = V (t) − V (t0 )
t0
⎧ & '⎫
N ⎨ N 2 ⎬
αi T ji
≤ −δ̄di q̇i 22 − δri ri 22 + ai j q̇i 22 + q̇ j 22 ,
⎩ 2 2αi ⎭
i=1 j=1
(10.39)
where αi is any positive constant. Since evidently

2 2

N
N
T ji
N
N
T ij
ai j q̇ j 22 = a ji q̇i 22 , (10.40)
i=1 j=1
2αi i=1 j=1
2α j
then (10.39) becomes

⎧ & '⎫
N ⎨

N 2 ⎬
αi T ij
V (t) − V (t0 ) ≤ −δ̄di q̇i 22 − δri ri 22 + q̇i 22 ai j + a ji .
⎩ 2 2α j ⎭
i=1 j=1
(10.41)
By choosing δ̄di according to

& 2 '

N
αi T ij
δ̄di ≥ δdi + ai j + a ji , (10.42)
j=1
2 2α j
where δdi is a positive constant, one gets

N

V (t) + δdi q̇i 22 + δri ri 22 ≤ V (t0 ). (10.43)
i=1
Since this relationship is valid only for x ∈ D, it is necessary to guarantee that x will
never leave this region. After (10.23) and (10.43) one must have
λ1 x2 ≤ V (t) ≤ V (t0 ) ≤ λ2 x(t0 )2 , (10.44)
which implies that x is bounded by

(
λ2
x ≤ x(t0 ). (10.45)
λ1
Thus, in order to enforce x ∈ D for all time it should hold

( (
λ2 λ1
x(t0 ) < ⇒ x(t0 ) < . (10.46)
λ1 λ2
Satisfying (10.46) makes (10.43) valid for all time t ≥ t0 , which implies that
• q̇i , ri ∈ L2 ∀ i = 1, . . . , N
• V (t) ∈ L∞
After (10.17), the latter fact ensures that
• q̇i , qi − q j , ri , zi ∈ L∞ ∀ i, j = 1, . . . , N
From (10.11) one necessarily has
• żi ∈ L∞ ∀ i = 1, . . . , N
and consequently
• q̂˙ i ∈ L∞ ∀ i = 1, . . . , N
and from (10.14)
• ṙi ∈ L∞ ∀ i = 1, . . . , N
because only revolute joints are considered for simplicity. Since ri ∈ L2 ∩ L∞
Lemma 4.4 can be employed to show that
• ri → 0 ∀ i = 1, . . . , N
But from (10.11) this proves that
• zi , żi → 0, and q̂i → qi , q̂˙ i → q̇i ∀ i = 1, . . . , N
On the other hand, from (10.5) it is
qi − q̄ j = qi − q j (t − T ji (t)) = qi − q j + q j − q j (t − T ji (t)), (10.47)
where it has been shown that qi − q j is bounded. As for the second term, Schwartz’s
inequality, i.e.
⎡ t ⎤ 21 ⎡ t ⎤ 21
t
aT (θ )b(θ )dθ ≤ ⎣ a(θ )2 dθ ⎦ ⎣ b(θ )2 dθ ⎦ , (10.48)
t0 t0 t0
can be applied for any vector signals a, b. To do this consider

) )2
) t )
) )
) )
q j (t) − q j (t − T ji (t)) = )
2
q̇ j (θ )dθ ) , (10.49)
) )
) t−T ji (t) )
and set a = 1 and b = q̇ j to apply (10.48) to get
t
) )
q j − q j (t − T ji (t)) ≤ T ji (t)
2 )q̇ j (θ ))2 dθ
t−T ji (t)
t
) )
≤ T ji )q̇ j (θ ))2 dθ ≤ T ji q̇ j 2 , (10.50)
2
t0
where Assumption 10.2 and the fact that
t t
) ) ) )
)q̇ j (θ ))2 dθ ≤ )q̇ j (θ ))2 dθ (10.51)
t−T ji (t) t0
have been taken into account. Since q̇ j ∈ L2 , it can be concluded that

• qi − q̄ j ∈ L∞ ∀ i, j = 1, . . . , N
In turn, after (10.9) and Property 3.7 it is
• q̈i ∈ L∞ ∀ i = 1, . . . , N
By taking into account this last fact and in view that q̇i ∈ L2 ∩ L∞ , Lemma 4.4 can
be employed to show that
• q̇i → 0 ∀ i = 1, . . . , N
This fact shows from (10.14) that
• ṙi → 0 ∀ i = 1, . . . , N
This in turn implies that
• z̈i → 0 ∀ i = 1, . . . , N
Now, by computing the derivative of (10.9)
d
H i (qi ) q̈ + Ḣ i (qi )q̈i + Ċ i (qi , q̇i , q̈i )q̇i + C i (qi , q̇i )q̈i
dt i
N
+ K vi q̈i − di z̈i + pi ai j (q̇i − (1 − Ṫ ji (t))q̇ j (t − T ji (t))) = 0,
j=1
(10.52)
it is possible to show that dtd q̈i must also be bounded ∀ i = 1, . . . , N since H i (qi ) is
full rank and Ṫ ji (t) is bounded by Assumption 10.2. Furthermore, one can conclude
from Lemma 4.5 (Barbalat’s Lemma) that
• q̈i → 0 ∀ i = 1, . . . , N
because
∞
q̈i (θ )dθ = q̇i (∞) − q̇i (t0 ) = −q̇i (t0 ). (10.53)
t0
Since q̇i , żi → 0, the equilibrium point of (10.9) satisfies

N
pi ai j (qi − q̄ j ) = 0. (10.54)
j=1
The fact that velocities are zero at the equilibrium makes q̄ j = q j (t − T ji (t)) ≡
q j (t), and the following holds

N
pi ai j (qi − q j ) = 0, (10.55)
j=1
which can be rewritten as

N
ai j qi − ai1 q1 − · · · − aii qi − · · · − ai N q N = 0. (10.56)
j=1
According with (10.1), li j = −ai j unless i = j, for which case aii = 0 but li j =
N
k=1 aik , which means that from (10.56) one gets
li1 q1 + · · · + li N q N = 0, (10.57)
for i, j = 1, . . . , N . In a compact form one can write

⎡ ⎤
q1
⎢ ⎥
[li1 I n · · · li N I n ] ⎣ ... ⎦ = 0. (10.58)
qN
Define
⎡ ⎤
q1
⎢ ⎥
q = ⎣ ... ⎦ (10.59)
qN
to use the standard Kronecker product ⊗ as

⎡ ⎤
l11 I n · · · l1N I n
⎢ .. ⎥ q = 0.
(L ⊗ I n )q = ⎣ ... . ⎦ (10.60)
l N 1In · · · l N N In
Recall now that the Laplacian L matrix has almost only positive eigenvalues with
the exception of one single zero eigenvalue and that any eigenvector associated to
that single zero eigenvalue is given by α1 N ∈ R N where α is any real value different
from zero and 1 N is a column vector filled with ones. This means that
(L ⊗ I n )q = 0 (10.61)
can hold if and only if
q1 = · · · = q N ≡ qc , (10.62)
with qc a constant vector, precisely because L has one single zero eigenvalue asso-
ciated to the eigenvector 1n .
Leader follower consensus problem (LFCP)
The constant vector qc in (10.62) can in fact be any constant vector. This represents
a serious drawback in practical situations because physically a robot manipulator
can only reach those positions in its work space. Therefore, it is more desirable that
the network can be regulated at a given constant leader pose qle ∈ Rn . Of course,
it makes little sense to give this information to all of the robots of the network, so
that qle is only available to a certain set of them, i.e. instead of (10.3) one has to
accomplish
lim qi (t) = qle and lim q̇i (t) = 0. (10.63)

t→∞ t→∞
It turns out that the control law (10.8) needs a minimal change to achieve this goal,
since it has to be modified as

N
τ i = gi (qi ) − pi ai j (qi − q̄ j ) − di q̂˙ i − pi bli (qi − qle ), (10.64)
j=1
where bli is positive for n l < N manipulators, while the rest of them are zero. This
modification needs also to be included in the observer, so that (10.7) needs to be
changed as
⎧ ⎫
⎨N ⎬
ξ̇ i = K di zi zi − pi H i−1 (qi ) ai j (qi − q̄ j ) + bli (qi − qle ) . (10.65)
⎩ ⎭
j=1
To analyze the stability properties of the new scheme an extra error definition only
for those robots where bli > 0 is given by
ei = qi − qle , (10.66)
while at the same time the definition of xi in (10.16) has to be expanded as

⎡ ⎤
ei
⎢ ei1 ⎥
⎢ ⎥
⎢ .. ⎥
⎢ . ⎥
⎢ ⎥
xi = ⎢ ei N ⎥ , (10.67)
⎢ ⎥
⎢ q̇ ⎥
⎢ i ⎥
⎣ ri ⎦
zi
while the definition of x in (10.22) is still valid. Finally, the positive definite functions
Vi in (10.17) have also to be modified as
1
N
1 T bli
Vi = q̇i H i (qi )q̇i + |qi − qle |2 + ai j qi − q j 2
2 pi 2 4 j=1
1 T 1
+ ri ri + γi ziT −1
zi zi . (10.68)
2 2
Note that (10.18) still holds, but with

λhi 1 1 bli 1 γi
λi1 = min , min ai j , , min , (10.69)
2 pi 4 j∈Ni 2 2 2 λmax (zi )
and

λHi 1 1 bli 1 γi
λi2 = max , max ai j , , max , . (10.70)
2 pi 4 j∈Ni 2 2 2 λmin (zi )
It is now possible to use Vi in (10.68) together with V (x) in (10.21) to arrive by direct
computation to the very same conclusions as before, i.e.
• q̇i , ri , qi − q̄ j , q̈i ∈ L∞
• q̇i , ri , zi , żi → 0, ṙi → 0, z̈i → 0
• qi − qle ∈ L∞
Most important, this can be done under the same stability conditions (10.35)–(10.37)
and (10.42) for i, j = 1, . . . , N . However, equation (10.9) changes to
H i (qi )q̈i + C i (qi , q̇i )q̇i + K vi q̇i − di żi

N
+ pi ai j (qi − q̄ j ) + pi bli (qi − qle ) = 0 (10.71)
j=1
when necessary. As before, the computation of the derivative of this equation allows
to arrive to the conclusion that
• q̈i → 0 ∀ i = 1, . . . , N
As to the equilibrium point instead of (10.55) one has

N
ai j (qi − q j ) + bli (qi − qle ) = 0. (10.72)
j=1
The second term can be expressed for ∀ i = 1, . . . , N as

⎡ ⎤⎡ ⎤
bl1 I n ··· O q1 − qle
⎢ .. .. .. ⎥ ⎢ .. ⎥
⎣ . . . ⎦⎣ . ⎦. (10.73)
O · · · blN I n q N − qle
By defining B = diag (bl1 , . . . , blN ) with bli = 0 when necessary and using q
in (10.59), the last equation can be rewritten using the Kronecker product as
(B ⊗ I n )(q − (1 N ⊗ qle )). (10.74)
Therefore, using this expression and (10.60) one can write (10.72) as
(B ⊗ I n )(q − (1 N ⊗ qle )) + (L ⊗ I n )q = 0. (10.75)
Recall as explained before that L1 N = 0, which also implies that (L ⊗ I n )(1 N ⊗

qle ) = 0, so that equivalently one has
(B ⊗ I n )(q − (1 N ⊗ qle )) + (L ⊗ I n )q − (L ⊗ I n )(1 N ⊗ qle ) = 0, (10.76)

or
(Ll ⊗ I n )(q − (1 N ⊗ qle )) = 0, (10.77)
by using the properties of the Kronecker product and by defining
Ll = B + L. (10.78)
But since L is a positive semidefinite matrix with one single zero eigenvalue, the
addition of B is enough to make the matrix Ll symmetric and positive definite. This
means that (10.77) can hold if and only if
q − (1 N ⊗ qle ) = 0, (10.79)
which in turn means
qi ≡ qle , (10.80)
for i = 1, . . . , N . This shows that (10.63) has been achieved.
Leaderless Consensus Problem

Tuning guide
To implement the LCP algorithm described in Sect. 10.2, one needs only the observer
equations (10.6)-(10.7) and the control law (10.8). The following suggestion can be
useful when implementing the controller.
• Set the interconnection gains pi > 0.
• Add damping by setting the di gains to satisfy (10.35). Note that the larger these
gains, the more stable will be the robots’ motion, but at the expense of making
them stiffer.
Experiments
Two experiments are carried out to test the LCP controller of Sect. 10.2, one with
constant time-delays in the communication channel and the other one with variable
time-delays. For the first case, different time-delays are considered for each robot, i.e.
T12 = T13 = 0.45 s, T21 = T23 = 0.65 s and T31 = T32 = 0.85 s. In the time-varying
delays case, there are considered T12 = T13 ∈ [0.4, 0.5] s, T21 = T23 ∈ [0.6, 0.7] s,
and T31 = T32 ∈ [0.8, 0.9] s. A plot of the time delays is shown in Fig. 10.2.
The interconnection constants are assumed to be ai j = 5, i = j. Each robot is
manually drove to an arbitrary initial position, aided by a gravity compensation
Fig. 10.2 Artificially implemented time-delays: robot 1 (up),robot 2 (middle), robot 3 (down)
Table 10.1 Chosen gains for the LCP controller

Gain Value
p1 , p2 , p3 0.1
d1 , d2 , d3 0.25
K d1 , K d2 , K d3 100I
z1 , z2 , z3 0.1I
torque, before running the controller. The chosen gains for the implementation of the
LCP controller for both constant and time-varying delays are shown in Table 10.1.
Constant time-delay
The time evolution of the joint positions for all three robots is displayed in Fig. 10.3.
From this graphic, it can be seen that the robots reach a consensus position in steady
state, after about 3 s. The joint velocities estimated by the observer along with the
ones obtained by a derivative filter are shown in Figs. 10.4, 10.5 and 10.6, for robots
1,2, and 3, respectively.
Fig. 10.3 LCP under constant time-delays, joint positions: robot 1 (—), robot 2 (—), robot 3 (—)
Fig. 10.4 LCP under constant time-delays, joint velocity estimation for robot 1: observer (—),
numerical (– –)
numerical (– -)
Time-varying delay
The time evolution of the joint positions for all three robots is shown in Fig. 10.7. As
in the constant delay case, the robots reach a consensus position after about 3 s. The
joint velocities estimated by the observer along with the ones obtained by a derivative
filter are shown in Figs. 10.8, 10.9 and 10.10, for robots 1, 2, and 3, respectively.
10.3.1 Leader-Follower Consensus Problem
Tuning guide
The required equations for the implementation of the LFCP scheme of Sect. 10.2 are
the observer equations (10.6) and (10.65), and the control law (10.64). To tune the
controller, the following suggestions might be useful.
• Set the interconnection gains pi > 0.
numerical (– –)
• Add damping by setting the di gains to satisfy (10.35). Note that the larger these
gains, the more stable will be the robots’ motion, but at the expense of making
them stiffer.
Experiments
In order to test the LFCP controller of Sect. 10.2, the same two scenarios as in the LCP
problem are considered, i.e. constant and variable time-delays in the communication
channel. For these case, the same delays of the LCP problem are considered, i.e. the
ones depicted in Fig. 10.2.
The interconnection constants are assumed to be ai j = 5, i = j. It is considered
that only the robot 1 receives information from the leader, by setting bl1 = 15. The
leader position is set to
T
qle = −30◦ 45◦ −90◦ .
Each robot is manually driven to an arbitrary initial position, aided by a grav-

ity compensation torque, before running the controller. The chosen gains for the
implementation of the LFCP controller for both constant and time-varying delays
are shown in Table 10.2.
Fig. 10.7 LCP, under variable time-delays, joint positions: robot 1 (—), robot 2 (—), robot 3 (—)
Table 10.2 Chosen gains for the LFCP controller

Gain Value
p1 , p2 , p3 0.1
d1 , d2 , d3 0.25
K d1 , K d2 , K d3 100I
z1 , z2 , z3 0.1I
Constant time-delay
The joint positions for all three robots are shown in Fig. 10.11. In this figure, it can
be seen that all the three robot positions approach to the leader reference in steady
state after about 3 s, though some oscillations are still present in joint 1 of robots 1
and 3. The joint velocities estimated by the observer along with the ones obtained
by a derivative filter are shown in Figs. 10.12, 10.13, and 10.14, for robots 1,2, and
3, respectively.
Fig. 10.8 LCP under variable time-delays, joint velocity estimation for robot 1: observer (—),
numerical (– –)
Time-varying delay
The time evolution of the joint positions for all three robots is shown in Fig. 10.15. As
in the constant delay case, the robots approach to the leader position after about 3 s,
with less oscillations than in the constant delay case. The joint velocities estimated
by the observer along with the ones obtained by a derivative filter are shown in
Figs. 10.16, 10.17, and 10.18, for robots 1,2, and 3, respectively.
numerical (– –)
numerical (– –)
Fig. 10.11 LFCP under constant time-delays, joint positions: robot 1 (—), robot 2 (—), robot 3 (—)
Fig. 10.12 LFCP under constant time-delays, joint velocity estimation for robot 1: observer (—),
numerical (– –)
numerical (– -)
numerical (– –)
Fig. 10.15 LFCP, under variable time-delays, joint positions: robot 1 (—), robot 2 (—), robot 3 (—)
Fig. 10.16 LFCP under variable time-delays, joint velocity estimation for robot 1: observer (—),
numerical (– –)
numerical (– –)
400
200
0
-200
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
100
0
-100
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
400
200
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
numerical (– –)
References
Abdessameud A, Tayebi A, Polushin IG (2016) Leader-follower synchronization of Euler-Lagrange

systems with time-varying leader trajectory and constrained discrete-time communication. IEEE
Trans Autom Control. https://doi.org/10.1109/TAC.2016.2602326
Aldana CI, Romero E, Nuño E, Basañez L (2015) Pose consensus in networks of heterogeneous
robots with variable time delays. Int J Robust Nonlinear Control 25(14):2279–2298
Anderson RJ, Spong MW (1989) Bilateral control of teleoperators with time delay. IEEE Trans
Autom Control 34(5):494–501
Arteaga-Pérez MA, Kelly R (2004) Robot control without velocity measurements: new theory and
experimental results. IEEE Trans Robot Autom 20(2):297–308
Arteaga-Pérez MA, Nuño E (2018) Velocity observer design for the consensus in delayed robot
networks. J Franklin Instit 355:6810–6829. https://doi.org/10.1016/j.jfranklin.2018.07.001
Nuño E (2016) Consensus of Euler-Lagrange systems using only position measurements’. IEEE
Trans Control Network Syst 2620806:2016. https://doi.org/10.1109/TCNS
Nuño E, Basañez L, Ortega R, Spong MW (2009) Position tracking for non-linear teleoperators
with variable time delay. Int J Robot Res 28(7):895–910
Nuño E, Basañez L, Ortega R (2011) Passivity-based control for bilateral teleoperation: a tutorial.
Automatica 47(3):485–495
Nuño E, Sarras I, Basañez L (2013) Consensus in networks of nonidentical Euler-Lagrange systems
using P+d controllers. IEEE Trans Robot 29(6):1503–1508
Ren W, Cao Y (2011) Distributed coordination of multi-agent networks: emergent problems, models,
and issues. Springer, London, Great Britain
Sarras I, Nuño E, Basañez L, Kinnaert M (2016) Position tracking in delayed bilateral teleoperators
without velocity measurements. Int J Robust Nonlinear Control 26(7):1437–1455
Xi J, He M, Liu H, Zheng J (2016) Admissible output consensualization control for singular multi-
agent systems with time delays. J Franklin Inst 353(16):4074–4090
Part III
Different Testbeds and the Adaptation
of Industrial Robots for Practical
Implementation
Chapter 11
The Robot CRS 465
Abstract Parts I and II deal mainly with analytical results for the modeling and
control of robot manipulators. Although the main algorithms are expressed in a rather
generic way so that they are useful for any robot manipulator, in practice each of
them has its own particularities. The last part of the book deals with the mathematical
description of some of the robots present at the Laboratory for Robotics of the School
of Engineering at the National Autonomous University of Mexico, UNAM. This
chapter is devoted to developing the kinematic and dynamic models of the robot
A465 by CRS Robotics.
11.1 Characteristics of the Robot CRS A465
The robot A465 is a serial-link manipulator manufactured by the Canadian company

CRS Robotics. It is shown in Figs. 1.1 and 11.1. The main characteristics of the CRS
A465 are the following:
• 6 degrees of freedom industrial robot made up of rigid links and revolute joints.
• The actuators are DC motors equipped with harmonic drive gears. The reduction
gears provide high input torques.
• Each joint is equipped with a high-resolution incremental encoder that measures
the joint position.
• The last three joints form a spherical wrist.
• The maximum payload is 2 kg.
• The total weight of the robot is 31 kg.
11.2 Kinematics of the Robot A465
To be able to work with the A465, the first step is to describe its direct kinematic as
explain in Sect. 2.2. First of all, seven coordinate frames are assigned according to the
Denavit-Hartenberg Algorithm 1 (see Fig. 11.2). Then, Algorithm 2 is employed to
https://doi.org/10.1007/978-3-030-85980-0_11
314 11 The Robot CRS 465
Fig. 11.1 Serial-link

manipulator A465
Table 11.1 Denavit-Hartenberg parameters (θi , αi , di , ai ) for the robot CRS A465
Joint θi (◦ ) αi (◦ ) di (m) ai (m)
1 q1 90 0.33 0
2 q2 0 0 0.305
3 q3 90 0 0
4 q4 −90 0.33 0
5 q5 90 0 0
6 q6 0 0.076 0
compute the different homogeneous transformations, while the different parameters

are given in Table 11.1.
Based on Table 11.1 and (2.146), i.e.
⎡ ⎤
i−1
Ai = ⎢
⎣ 0
⎥,
sθi cθi di ⎦
0 0 0 1
the different homogenous transformation matrices associated with each coordinate

frame are given by
⎡ ⎤
c1 0 s1 0
⎢ s1 0 −c1 0⎥
0
A1 = ⎢
⎣0
⎥ (11.1)
1 0 d1 ⎦
0 0 0 1
11.2 Kinematics of the Robot A465 315
d4 d6
x2 .x3 x4 , x5 q5 x6
q4 q6
y2 z3 y4
z5 , z6
z2 .y3 z4 , y5 y6
y1 a2
q3
x1
z1
z0 d1
q2 y0
x0
q1
Fig. 11.2 Coordinate frames for the robot A465 assigned according to the Denavit-Hartenberg
Algorithm 1.
⎡ ⎤
c2 −s2 0 a2 c2
⎢ s2 c2 0 a 2 s2 ⎥
1
A2 = ⎢
⎣0
⎥ (11.2)
0 1 0 ⎦
0 0 0 1
⎡ ⎤
c3 0 s3 0
⎢ s3 0 −c3 0⎥
2
A3 = ⎢
⎣0
⎥ (11.3)
1 0 0⎦
0 0 0 1
⎡ ⎤
c4 0 −s4 0
⎢ s4 0 c4 0⎥
3 ⎢
A4 = ⎣ ⎥ (11.4)
0 −1 0 d4 ⎦
0 0 0 1
⎡ ⎤
c5 0 s5 0
⎢ s5 0 −c5 0⎥
4
A5 = ⎢
⎣0
⎥ (11.5)
1 0 0⎦
0 0 0 1
⎡ ⎤
c6 −s6 0 0
⎢ s6 c6 0 0⎥
5
A6 = ⎢
⎣0
⎥ (11.6)
0 1 d6 ⎦
0 0 0 1
where ci = cos(qi ) and si = sin(qi ) with i = 1, 2, . . . , 6. By taking into account

the previous equations, the homogeneous transformation 0 T 6 that relates the base
coordinate frame ox0 y0 z0 with the end-effector frame ox6 y6 z6 is given by
0
R 6 0 p6
0
T6 = = 0 A1 1 A2 2 A3 3 A4 4 A5 5 A6 . (11.7)
0T 1
The position of the end-effector can then be calculated as

⎡ ⎤
a2 c1 c2 + d4 c1 s12 + d6 (c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5 )
0
p6 = ⎣ a2 s1 c2 + d4 s1 s23 + d6 (s1 (c23 c4 s5 + s23 c5 ) − c1 s4 s5 ) ⎦ (11.8)
d1 + a2 s2 − d4 c23 + d6 (s23 c4 s5 − c23 c5 )
where c23 = cos(q2 + q3 ) and s23 = sin(q2 + q3 ). On the other hand, the orientation
of the robot 0 R6 is expressed as follows
0
R6 = 0 R3 3 R6 (11.9)
where the rotation matrices 0 R3 and 3 R6 are given by

⎡ ⎤
c1 c23 s1 c1 s23
0
R3 = ⎣ s1 c23 −c1 s1 s23 ⎦ (11.10)
s23 0 −c23
⎡ ⎤
c4 c5 s6 − s4 s6 −c4 c5 s6 − s4 c6 c4 s5
3
R6 = ⎣ s4 c5 c6 + c4 s6 −s4 c5 s6 + c4 c6 s4 s5 ⎦ . (11.11)
−s5 c6 s5 c6 c5
Notice that the rotation matrix 3 R6 matches the rotation matrix obtained with the
Euler angles Z Y Z , i.e. the joints q4 , q5 and q6 constitute a set of Euler angles Z Y Z
with respect to the coordinate frame x3 y3 z 3 .
11.3 Dynamics of the Robot A465
In order to get a dynamic model, it is necessary to compute different moments and

products of inertia with respect to new coordinate frames fixed at the different centers
of mass of the links. The first challenge, however, is to deal with the evident lack of
symmetry of the links as well as with a non-uniform mass density. For that reason,
11.3 Dynamics of the Robot A465 317
x
y
r
y a
z
z
c x
b
Fig. 11.3 Inertia tensor of a rectangular prism and cylinder used to approximate the links of the
robot A465
the links are approximated by rectangular prisms and cylinders with uniform mass
density as shown in Fig. 11.3. Recall that the inertia tensors of a rectangular prism
and of a cylinder, denoted by I p and I c respectively, are given by
⎡ ⎤
1
12
m(b2 + c2 ) 0 0
Ip = ⎣ 0 1
12
m(a 2
+ c2 ) 0 ⎦
0 0 1
12
m(a + b )
2 2
⎡ ⎤
1
12
m(3r 2 + 2 ) 0 0
Ic = ⎣ 0 1
12
m(3r 2 + 2 ) 0 ⎦
1
0 0 2
mr 2
where m is the mass of the object and the geometric parameters a, b, c, d and r are
shown in Fig. 11.3.
The inertia matrix of the manipulator is given by (3.27), i.e.
6
H(q) = m i J Tvci (q) J vci (q) + J Tωci (q)0 Ri I i 0 RiT J ωci (q) , (11.12)
i=1
where the Jacobian matrices J vci (q) ∈ R3×6 can be computed as follows

J vc1 (q) = z 0 × 0 r̄ 1 O 3×5

J vc2 (q) = z 0 × 0 r̄ 2 z 1 × (0 r̄ 2 − 0 p1 ) O 3×4

J vc3 (q) = z 0 × 0 r̄ 3 z 1 × (0 r̄ 3 − 0 p1 ) z 2 × (0 r̄ 3 − 0 p2 ) O 3×3
J vc4 (q) = z 0 × 0 r̄ 4 z 1 × (0 r̄ 4 − 0 p1 ) z 2 × (0 r̄ 4 − 0 p2 ) · · ·

z 3 × (0 r̄ 4 − 0 p3 ) O 3×2
J vc5 (q) = z 0 × 0 r̄ 5 z 1 × (0 r̄ 5 − 0 p1 ) z 2 × (0 r̄ 5 − 0 p2 ) · · ·

z 3 × (0 r̄ 5 − 0 p3 ) z 4 × (0 r̄ 5 − 0 p4 ) 0
J vc6 (q) = z 0 × 0 r̄ 6 z 1 × (0 r̄ 6 − 0 p1 ) z 2 × (0 r̄ 6 − 0 p2 ) · · ·

z 3 × (0 r̄ 6 − 0 p3 ) z 4 × (0 r̄ 6 − 0 p4 ) z 5 × (0 r̄ 6 − 0 p5 )
T
where z 0 = 0 0 1 and z j = 0 R j z 0 ; these vectors can be extracted from the homo-
geneous transformation matrix 0 A j with j = 1, . . . , 6. The vectors 0 r̄ j ∈ R3 denotes
the position of the center of mass of the jth link and 0 p j is the position vector of the
origin of the frame j. The Jacobian matrices associated with the angular velocities
are given by
⎡ ⎤
0 0 ··· 0
J ωc1 = ⎣0 0 ··· 0⎦
1 0 ··· 0
⎡ ⎤
0 s1 · · · 0
J ωc2 = ⎣ 0 −c1 · · · 0⎦
1 0 ··· 0
⎡ ⎤
0 s1 s1 · · · 0
J ωc3 = ⎣ 0 −c1 −c1 · · · 0⎦
1 0 0 ··· 0
⎡ ⎤
0 s1 s1 c1 s23 · · · 0
J ωc4 = ⎣ 0 −c1 −c1 s1 s23 · · · 0 ⎦
1 0 0 −c23 · · · 0
⎡ ⎤
0 s1 s1 c1 s23 s1 c4 − c1 c23 s4 0
J ωc5 = ⎣ 0 −c1 −c1 s1 s23 −s1 c23 s4 − c1 c4 0 ⎦
1 0 0 −c23 −s23 s4 0
⎡ ⎤
0 s1 s1 c1 s23 s1 c4 − c1 c23 s4 s5 (s1 s4 + c1 c23 c4 ) + c1 s23 c5
J ωc6 = ⎣ 0 −c1 −c1 s1 s23 −s1 c23 s4 − c1 c4 s1 s23 c5 − s5 (c1 s4 − s1 c23 c4 ) ⎦ .
1 0 0 −c23 −s23 s4 s23 c4 s5 − c23 c5
To proceed, let us compute the center of mass and inertia tensor of the robot’s
links. The position of the center of mass of each link can be computed using the
homogeneous transformation matrices 0 A1 . . . . , 0 A6 as follows
0
r̄ j = 0 A j j r̄ j , j = 1, . . . , 6 (11.13)
T T
where 0 r̄ j = 0 r̄ j 1 ∈ R4 and j r̄ j = j r̄ j 1 ∈ R4 . The constant vector j r̄ j ∈
R3 is the position of the link’s center of mass expressed with respect to frame j.
Now, it is possible to begin with the computation of the different moments of
inertia. First of all, link 1 is approximated according to Fig. 11.4 and where the
coordinate frames ox0 y0 z0 and ox1 y1 z1 are shown. The total mass of the link is given
Fig. 11.4 Coordinate frames y1

and geometry of link 1 for
the robot A465
z1
m11 m13
m12
d1
c1
z0
y0
y11 11 13 y13

r11 r13
z11 m11 z13 m13
yc1
c11 c13
zc1
r12 y12
12
c12
z12
m12
Table 11.2 Parameters of the link 1

Parameter Value Units
c11 0.2 m
c12 0.2 m
c13 0.2 m
r11 0.076 m
r12 0.1 m
r13 0.076 m
c1 0.295 m
11 0.257 m
12 0.05 m
13 0.257 m
m 11 3.0 kg
m 12 5.0 kg
m 13 3.0 kg
by m 1 = m 11 + m 12 + m 13 . To simplify calculations it is assumed that the center of

mass of the link is located along the y1 axis.
The different parameters of link 1 are summarized in Table 11.2. It is worthy to
point out that these parameters are just approximated values since the actual ones
are not provided by the manufacturer.
From Fig. 11.4, the position of the center of mass expressed with respect to frame 1
is given by
⎡ ⎤
0
1
r̄ 1 = ⎣ c1 − d1 ⎦ . (11.14)
0
By using Steiner’s theorem (parallel-axis theorem) the diagonal elements of the

inertia tensor are computed as follows
1 2 1 2
Ixx1 = m 11 r11 + c11
2
+ 2m 11 211 + (d1 − c1 )2 + 3r12 + c122
6 12
1 2 1
Iyy1 = m 11 3r11 + c11 + m 12 r12
2 2
6 2
1 2
Izz1 = m 11r11 + 2m 11 d1 − 2c1 + m 12 3r12
2
+ c122
+ m 12 212
12
where it is assumed that m 11 = m 13 , r11 = r13 and 11 = 13 . The parameters of the
first link are shown in Table 11.2.
The second link is approximated by a rectangular prism. Figure 11.5 shows the
coordinate frames and parameters of link 2. It is assumed that the center of mass is
located along the x2 -axis, i.e.
Fig. 11.5 Coordinate frames x2

and geometry of link 2 for y2
the robot A465
z2
a2
c2
y1
z1
x2
y2
b2
z2
xc2
yc2
zc2 y1 2
z1
c2
⎡ ⎤
c2 − a2
2
r̄ 2 = ⎣ 0 ⎦. (11.15)
0
The diagonal elements of the inertia tensor computed with respect to frame 2 are
given by
1
Ix x2 = m 2 b22 + c22
12
1
I yy2 = m 2 c22 + 22 + m 2 22
12
1
Izz2 = m 2 b22 + 22 + m 2 22
12
Table 11.3 Parameters of the links 2 and 3

Link 2 Link 3
Parameter Value Units Parameters Value Units
b2 0.6 m r3 0.051 m
c2 0.6 m 3 0.12 m
2 0.26 m c3 0.04 m
c2 0.155 m m3 4.0 kg
m2 5.0 kg – – –
where the numerical values are shown in Table 11.3.

The third link is approximated by a cylinder and it is assumed that the position of
its center of mass has only a component in the z 3 -axis (see Fig. 11.6). The position
Fig. 11.6 Coordinate frames Top view

the robot A465
z3
y3
c3
x3 xc3
3
r3
c3 zc3
y3 yc3
of the center of mass expressed with respect to the frame 3 is given by

⎡ ⎤
0
3
r̄ 3 = ⎣ 0 ⎦ . (11.16)
c3
The diagonal elements of the inertia tensor I 3 are given by
1 2
Ix x3 = m 3 3r3 + 23 + m 3 2c3
2
1
I yy3 = m 3 3r32 + 23 + m 3 2c3
2
1
Izz3 = m 3r32 .
2
Another reasonable assumption is to locate the center of mass at the origin of the
frame 3, which leads to c3 = 0. The parameters of the third link are given in
Table 11.3.
Top view
y3 y4
z3 c4 z4
d4
4
yc4 r4
y3
z3 zc4
Fig. 11.7 Coordinate frames and geometry of link 4 for the robot A465
Fig. 11.8 Coordinate frames

Top view
the robot A465
y4 z5
c5
y5
5
x5 xc5
z5 r5
zc5
y5 yc5
The coordinate frames and the different parameters of the fourth link are shown
in Fig. 11.7. In this case, the position of the center of mass is given by
⎡ ⎤
0
4
r̄ 4 = ⎣ d4 − c4 ⎦ . (11.17)
0
The numerical values of the parameters of link 4 are given in Table 11.4. Since it is
approximated by a cylinder, the diagonal elements of its inertia tensor are
1
Ix x4 = m 4 3r42 + 24 + m 4 (d4 − c4 )2
12
1
I yy4 = m 4 r42
2
1
Izz4 = m 4 3r42 + 24 + m 4 (d4 − c4 )2 .
12
Figure 11.8 shows the coordinate frame and the different parameters of link 5.
Similar to links 3 and 4, the fifth link is approximated by a cylinder and the position
of its center of mass expressed with respect to frame 5 is given by
Table 11.4 Parameters of links 4, 5 and 6 for the robot A465

Link 4 Link 5 Link 6
Parameter Value Units Parameter Value Units Parameter Value Units
r4 0.051 m r5 0.051 m r6 0.0335 m
4 0.259 m 5 0.11 m 6 0.036 m
c4 0.24 m c5 0.015 m c6 0.0785 m
m4 2.0 kg m5 0.5 kg m6 0.2 kg
x5
x6
y5 z5
c6 d6
z6
y6
Fig. 11.9 Coordinate frames and geometry of link 6 for the robot A465.
⎡ ⎤
0
5
r̄ 5 = ⎣ 0 ⎦ . (11.18)
c5
The elements of the inertia tensor of the fifth link are given by
1
Ix x5 = m 5 3r52 + 25 + m 5 2c5
12
1
I yy5 = m 5 3r52 + 25 + m 5 2c5
12
1
Izz5 = m 5r52 .
2
The numerical values of the parameters are shown in Table 11.4.
The last link is approximated by a cylinder as shown in Fig. 11.9.
The position of the center of mass of the sixth link is given by
⎡ ⎤
0
6
r̄ 6 = ⎣ 0 ⎦. (11.19)
c6 − d6
On the other hand, the diagonal elements of the inertia tensor are
1
Ix x6 = m 6 r62 + 26 + m 6 (c6 − d6 )2
12
1
I yy6 = m 6 r62 + 26 + m 6 (c6 − d6 )2
12
1
Izz6 = m 6r62 .
2
The different parameters of link 6 are given in Table 11.4. Finally, the elements of
the inertia matrix H(q) ∈ R6×6 can be computed as
H11 = m 2 2c2 c22 + m 3 (a2 c2 + c3 s23 )2 + m 4 (a2 c2 + c4 s23 )2

+ m 5 ((a2 c2 + c5 s23 c5 + d4 s23 + c5 c23 c4 s5 )2 + 2c5 s24 s25 )
+ m 6 ((a2 c2 + c6 s23 c5 + d4 s23 + c6 c23 c4 s5 )2 + 2c6 s24 s25 )
+ I yy1 + Ix x2 s22 + I yy2 c22 + Ix x3 s223 + Izz3 c223 + Ix x4 s223 c24
+ I yy4 c223 + Izz4 s223 s24 + Ix x5 (s23 c4 c5 + c23 s5 )2 + I yy5 s223 s24
+ Izz5 (s23 c4 s5 − c23 c5 )2 + Ix x6 (s23 c4 c5 c6 + c23 s5 c6 − s23 s4 s6 )2
+ I yy6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 )2 + Izz6 (s23 c4 s5 − c23 c5 )2
H12 = m 5 c5 s4 s5 (c23 (d4 + c5 c5 ) − a2 s2 − c5 s23 c4 s5 )
+ m 6 c6 s4 s5 (c23 (d4 + c6 c5 ) − a2 s2 − c6 s23 c4 s5 )
+ s23 c4 s4 (I x x4 − Izz4 ) + I x x5 s4 c5 (s23 c4 c5 + c23 s5 )
+ I x x6 (s23 c4 c5 c6 + c23 s5 c6 − s23 s4 s6 )(s4 c5 c6 + c4 s6 )
+ I yy6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 )(s4 c5 s6 − c4 c6 )
+ Izz6 s4 s5 (s23 c4 s5 − c23 c5 ) + Izz5 s4 s5 (s23 c4 s5 − c23 c5 )
− I yy5 s23 s4 c4
H13 = −m 5 c5 s4 s5 (c5 s23 c4 s5 − c23 (d4 + c5 c5 )) − m 6 2c6 s4 s5 s23 c4 s5
+ m 6 c6 c23 s4 s5 (d4 + c6 c5 ) + I x x5 s4 c5 (s23 c4 c5 + c23 s5 )
+ (I x x4 − Izz4 )s23 c4 s4 − I yy5 s23 s4 c4 + Izz5 s4 s5 (s23 c4 s5 − c23 c5 )
+ I yy6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 )(s4 c5 s6 − c4 c6 )
+ Izz6 s4 s5 (s23 c4 s5 − c23 c5 )
H14 = −m 5 c5 s5 (c5 c23 s5 + a2 c2 c4 + s23 c4 (d4 + c5 c5 )) − I yy4 c23
m 6 c6 s5 (c6 c23 s5 + a2 c2 c4 + s23 c4 (d4 + c6 c5 ))
− I x x5 s5 (s23 c4 c5 + c23 s5 ) + Izz5 c5 (s23 c4 s5 − c23 c5 )
− I x x6 s5 c6 (s23 c4 c5 c6 + c23 s5 c6 − s23 s4 s6 )
− I yy6 s5 s6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 )
+ Izz6 c5 (s23 c4 s5 − c23 c5 )
H15 = −m 5 c5 s4 (s23 (c5 + d4 c5 ) + a2 c2 c5 ) − I yy5 s23 s4
− m 6 c6 s4 (s23 (c6 + d4 c5 ) + a2 c2 c5 )
+ I x x6 s6 (s23 c4 c5 c6 +c23 s5 c6 − s23 s4 s6 )

− I yy6 c6 (s23 c4 c5 s6 + c23 s5 s6 + s23 s4 c6 )
H16 = Izz6 (s23 c4 s5 − c23 c5 )
H22 = m 2 2c2 + (m 5 + m 6 )a22 + m 3 (a22 + 2c3 + 2a2 c3 s3 ) + m 4 (a22 + 2c4 + 2a2 c4 s3 )
+ m 5 ((s5 c4 c5 )2 + (d4 + c5 c5 )2 + 2a2 s3 (d4 + c5 c5 + 2c5 a2 c3 c4 s5 ))
+ m 6 ((s5 c4 c6 )2 + (d4 + c5 c6 )2 + 2a2 s3 (d4 + c5 c6 ) + 2c6 a2 c3 c4 s5 )
+ Izz2 + I yy3 + I x x4 s24 + Izz4 c24 + I x x5 s24 c25 + I yy5 c24 + Izz5 s24 s25
+ I x x6 (s4 c5 c6 + c4 s6 )2 + I yy6 (s4 c5 s6 −c 4c6 )2 + Izz6 s24 s25
H23 = m 3 c3 (c3 + a2 s3 ) + m 4 c4 (c4 + a2 s3 ) + m 5 (d4 + c5 c5 )2 + I yy3
+ m 5 (a2 s3 (d4 + c5 c5 ) + (c5 c4 s5 )2 + c5 a2 c3 c4 s5 )) + m 6 (d4 + c6 c5 )2
+ m 6 (a2 s3 (d4 + c6 c5 ) + (c c4 s5 )2 + c6 a2 c3 c4 s5 ) + I x x4 s24 + Izz4 c24
+ Izz4 c24 + I x x5 s24 c25 + I yy5 c24 + Izz5 s24 s25 + I x x6 (s4 c5 c6 + c4 s6 )2
+ I yy6 (s4 c5 s6 − c4 c6 )2 + Izz6 s24 s25
H24 = −m 5 c5 s4 s5 (d4 + c5 c5 + a2 s3 ) − m 6 c6 s4 s5 (d4 + c6 c5 + a2 s3 )

+ (Izz5 − I x x5 )s4 c5 s5 − I x x6 s5 c6 (s4 c5 c6 + c4 s6 ) − I yy6 s5 s6 s4 c5 s6
+ I yy6 s5 s6 c4 c6 + Izz6 s4 s5 c5
H25 = m 5 c5 c4 (c5 + d4 c5 ) + m 5 c5 a2 (s3 c4 c5 + c3 s5 ) + m 6 2c6 c4
+ m 6 c6 d4 c4 c5 + m 6 c6 a2 (s3 c4 c5 + c3 s5 ) + I yy5 c4
+ I x x6 s6 (s4 c5 c6 + c4 s6 ) − I yy6 c6 (s4 c5 s6 − c4 c6 )
H26 = Izz6 s5 s4
H33 = m 3 2c3 + m 4 2c4 + m 5 (d4 + c5 c5 )2 + m 5 (c5 c4 s5 )2 + m 6 (d4 + c6 c5 )2
+ m 6 (c6 c4 s5 )2 + I yy3 + I x x4 s24 + Izz4 c24 + I x x5 s24 c25 + I yy5 c24 + Izz5 s24 s25
+ I x x6 (s4 c5 c6 + c4 s6 )2 + I yy6 (s4 c5 s6 − c4 c6 )2 + Izz6 s24 s25
H34 = −m 5 c5 s4 s5 (d4 + c5 c5 ) − m 6 c6 s4 s5 (d4 + c6 c5 ) + (Izz5 − I x x5 )s4 c5 s5
− I x x6 s5 c6 (s4 c5 c6 + c4 s6 ) − I yy6 s5 s6 (s4 c5 s6 − c4 c6 ) + Izz6 s4 s5 c5
H35 = m 5 c5 c4 (c5 + d4 c5 ) + m 6 c6 c4 (c6 + d4 c5 ) + I yy5 c4
+ I x x6 s6 (s4 c5 c6 + c4 s6 ) − I yy62 c6 (s4 c5 s6 − c4 c6 )
H36 = Izz6 s4 s5
H44 = m 5 (c5 s5 )2 + m 6 (c6 s5 )2 + I yy4 + I x x5 s25 + Izz5 c25 + I x x6 s25 c26
+ I yy6 s25 s26 + Izz6 c25
H45 = (I yy6 − I x x6 )s5 c6 s6
H46 = Izz6 c5
H55 = m 5 2c5 + m 6 2c6 + I yy5 + I x x61 s26 + I yy6 c26
H56 = 0
H66 = Iz66
Recall that as explained in Sect. 3.2, the elements of the matrix C(q, q̇) are computed
as a function of the elements of H(q) and they are omitted here simplicity’s sake.
As to the gravity vector g(q) ∈ R6 , its elements are given by
g1 = 0
g2 = m 2 gc2 c2 + (m 3 + m 4 + m 5 + m 6 )ga2 c2 + (m 4 c4 + (m 5 + m 6 )d4 )gs23
+ (m 5 c5 + m 6 c6 )(s23 c5 + c23 c4 s5 )
g3 = (m 3 c3 + m 4 c4 + (m 5 + m 6 )d4 )gs23 + (m 5 c5 + m 6 c6 )g(s23 c5 + c23 c4 s5 )
g4 = −(m 5 c5 + m 6 c6 )gs23 s4 s5
g5 = (m 5 c5 + m 6 c6 )g(c23 s5 + c23 c4 s5 )
g6 = 0
where g = 9.8(m/s2 ) is the gravity constant. As it is mentioned at the beginning of

the chapter the robot A465 is equipped with DC motors and harmonic drive gears.
The main characteristics of these devices such as model, mechanical and electrical
constants, gear ratio, etc. are shown in Tables 11.5 and 11.6.
Table 11.5 Main characteristic of the DC motors for the robot A465
Motor Brand Model Gear ratio
Motor 1 Cleveland motion controls M H 3515 r1 = 100
Motor 5 Tamagawa T S3253N r5 = 100
Motor 6 Tamagawa T S908N 7 r6 = 101
Table 11.6 Parameters of the DC motors for the robot A465

Motor Mechanical constant, Electrical constant, Armature resistance,
K a (Nm/A) K b (V rad/s) Ra ()
Motor 1 0.14234 0.14229 0.84
Motor 2 0.14234 0.14229 0.84
Motor 3 0.14234 0.14229 0.84
Motor 4 0.0053 0.0534 2.7
Motor 5 0.0053 0.0534 2.7
Motor 6 0.0392 0.0392 6.9
Chapter 12
The Robot CRS 255
Abstract The robot A255 by CRS Robotics is a smaller industrial robot than the
A465 but with only five degrees of freedom. It owns also a special kinematic arrange-
ment which makes it an interesting case of study.
12.1 Characteristics of the Robot CRS A255
This chapter is devoted to developing the kinematics and dynamic models of the
robot manipulator A255 by CRS Robotics shown in Figs. 1.1 and 12.1.
The robot A255 is a serial-link manipulator manufactured by the Canadian com-
pany CRS Robotics. Its main characteristics are the following:
• Five degrees of freedom made up of rigid links and revolute joints.
• The actuators are DC motors equipped with harmonic drive gears.
• Each joint is equipped with a high resolution encoder that measures the joint
angular position.
• The nominal payload is 1 kg.
• The total weight of the robot is 17 kg.
12.2 Kinematics of the Robot A255
The robot A255 is an open-chain serial manipulator made up of five joints, labeled as
qi with i = 1, . . . , 5. The main difference between the robot A465 is that the joints
2, 3 and 4 of the robot A255 are remotely driven by timing belts. Therefore, the joint
angle qi is not affected by the previous joint angles qi−1 (i = 3, 4) and Algorithm 2
cannot be used. These three joints are measured with respect to the same axis of
rotation. However, it is still possible to employ the Denavit-Hartenberg Algorithm 1
to assign the coordinate frames for each joint. Figure 12.2 shows the result in doing
so.
https://doi.org/10.1007/978-3-030-85980-0_12
Fig. 12.1 Serial manipulator robot A255
a3 d5
y3 , x4
y2 q4 x5
q5
x2
z5
x3 , z4
z2 z3
y1 a2 y5
q3
x1
z1
z0 d1
q2 y0
x0
q1
Fig. 12.2 Coordinate frames of the robot A255 assigned according to the Denavit-Hartenberg
Algorithm 1
12.2 Kinematics of the Robot A255 331
Table 12.1 Kinematic parameters of the robot CRS A255

Joint θi (◦ ) αi (◦ ) di (m) ai (m)
1 q1 90 0.254 0
2 q2 – 0 0.254
3 q3 – 0 0.254
4 q4 + 90 90 0 0
5 q5 0 0.0508 0
The kinematic parameters of the robot are shown in Table 12.1. Although Algo-
rithm 2 could be used for some joints, all the homogeneous transformation matrices
are computed by inspection and they are given by
⎡ ⎤
c1 0 s1 0
⎢ s1 0 −c1 0⎥
0
A1 = ⎢
⎣0
⎥ (12.1)
1 0 d1 ⎦
0 0 0 1
⎡ ⎤
1 0 0 a2 c2
⎢0 1 0 a 2 s2 ⎥
1
A2 = ⎢
⎣0
⎥ (12.2)
0 1 0 ⎦
0 0 0 1
⎡ ⎤
1 0 0 a3 c2
⎢0 1 0 a 3 s3 ⎥
2
A3 = ⎢
⎣0
⎥ (12.3)
0 1 0 ⎦
0 0 0 1
⎡ ⎤
−s4 0 c4 0
⎢ c4 0 s4 0⎥
3
A4 = ⎢
⎣ 0
⎥ (12.4)
1 0 0⎦
0 0 0 1
⎡ ⎤
c5 −s5 0 0
⎢ s5 c5 0 0⎥
4
A5 = ⎢
⎣0
⎥. (12.5)
0 1 d5 ⎦
0 0 0 1
By taking into account the previous equations the homogeneous transformation

matrix 0 T 5 that relates the base frame ox0 y0 z0 with the end-effector frame ox5 y5 z5
is given by
0
R 5 0 p5
0
T 5 = A1 A2 A3 A4 A5 =
0 1 2 3 4
, (12.6)
0T 1
where the position of the end-effector 0 p5 is given by

⎡ ⎤
c1 (a2 c2 + a3 c3 + d5 c4 )
0
p5 = ⎣ s1 (a2 c2 + a3 c3 + d5 c4 ) ⎦ (12.7)
d1 + a2 s2 + a3 s3 + d5 s4
and its orientation by

⎡ ⎤
s1 s5 − c1 s4 c5 s1 c5 + c1 s4 s5 c1 c4
0
R5 = ⎣ −c1 s5 − s1 s4 c5 s1 s4 s5 − c1 c5 s1 c4 ⎦ (12.8)
c4 c5 −c4 s5 s4
where ci = cos(qi ) and si = sin(qi ) with i = 1, 2, . . . , 5.
12.3 Dynamics of the Robot A255
Similar to the robot A465, the links of the robot A255 are approximated by regular
prisms, cylinders and solid disks. In this case, the inertia matrix is given after (3.27)
by
5
H(q) = m i J Tvci (q) J vci (q) + J Tωci (q)0 Ri I i 0 RiT J ωci (q) (12.9)
i=1
where the Jacobian matrices associated with the linear velocities J vci ∈ R3×5 are
computed as follows
∂ r̄ i
J vci (q) = , i = 1, . . . , 5 (12.10)
∂q
where r̄ i ∈ R3 is the center of mass of the i-th link, while the Jacobian matrices
associated to the angular velocities are given by
⎡ ⎤
0 0000
J ωc1 = ⎣0 0 0 0 0⎦
1 0000
⎡ ⎤
0 s1 0 0 0
J ωc2 = ⎣0 −c1 0 0 0 ⎦
1 0 000
⎡ ⎤
0 s1 s1 0 0
J ωc3 = ⎣ 0 −c1 −c1 0 0 ⎦
1 0 0 00
⎡ ⎤
0 s1 s1 s1 0
J ωc4 = ⎣ 0 −c1 −c1 −c1 0 ⎦
1 0 0 0 0
⎡ ⎤
0 s1 s1 s1 c1 c4
J ωc5 = ⎣ 0 −c1 −c1 −c1 s1 c4 ⎦ .
1 0 0 0 s4
The next step is to compute the center of mass and the inertia tensor of each link.
The first one is divided in three different bodies approximated by two cylinders and a
rectangular prism as shown in Fig. 12.3. For simplicity, it is assumed that the center
of mass of the link is located along the z 0 -axis, i.e.
y1 b13 y1
a13 m12
13
m13
d1 c12
x1
z1 m11 x1
z1
r11
z0 11 z0
c1 y0 y0
x0 x0
⎡ ⎤
0
0
r̄ 1 = ⎣ 0 ⎦ .
c1
On the other hand, by applying the parallel axis theorem, the diagonal elements of
inertia matrix I 1 are given by
2
1 2 2
2
1
Ixx1 = m 1i 3r1i2 + c1i + m 1i 21i + 2
m 13 a13 + c13
2
i=1
2 i=1
12
2
1 2 2
3
1
Iyy1 = m 1i 3r1i2 + c1i + m 1i 21i + 2
m 13 a13 + b13
2
i=1
2 i=1
12
2
1 1
Izz1 = m 1i r1i2 + m 13 b13
2
+ c13
2
+ m 13 213
i=1
2 12
The parameters of link 1 for the robot A255 are shown in the Table 12.2.
The second link is approximated by a rectangular prism. Figure 12.4 shows the
coordinate frames and the kinematic parameters associated with link 2. It is assumed
that the center of mass is along the y2 -axis, thus the vector position expressed with
respect to the base frame is given by
⎡ ⎤
c c2
0
r̄ 2 = ⎣ c s2 ⎦ .
d1
Table 12.2 Parameters of the link 1 for the robot A255

Parameter Value Units
c11 0.15 m
c12 0.15 m
c13 0.1 m
r11 0.05 m
r12 0.05 m
c1 0.2 m
11 0.1 m
12 0.1 m
13 0.08 m
m 11 2.0 kg
m 12 2.0 kg
m 13 3.0 kg
y2
y2
x2
z2 x2
z2
c2
a2
a2 y1 y1
b2 c2
z1 x1
z1 x1
The elements of the inertia matrix of the link 2 are given by
1 1
Ixx2 = m 2 a22 + b22 + m 2 a22 (12.11)
12 2
1
Iyy2 = m 2 b22 + c22 (12.12)
12
1 1
Izz2 = m 2 a22 + b22 + m 2 a22 , (12.13)
12 2
whose values can be seen in Table 12.3.
In a similar way, the third link is approximated by a rectangular prism and its
assumed that the center of mass is located along the x3 -axis as shown in Fig. 12.5.
This assumption yields to the following expression of its center of mass
⎡ ⎤
c1 (a2 c2 + c3 c3 )
0
r̄ 3 = ⎣ s1 (a2 c2 + c3 c3 ) ⎦ . (12.14)
d1 + a2 s2 + c3 s3
Table 12.3 Parameters of the links 2 and 3 for the second link for the robot A255
Link 2 Link 3
Parameter Value Units Parameter Value Units
a2 0.254 m a3 0.254 m
b2 0.1 m b3 0.1 m
c2 0.1 m c3 0.1 m
c2 0.15 m c3 0.2 m
m2 3 kg m3 2 kg
a3
y2
c3
y3
z2
x2
z3
x3
y2
a3
y3
x2
z2
b3
z3 x3
c3
In this case, the diagonal elements of the inertia matrix I 3 computed with respect to
frame ox3 y3 z3 are given by
1
Ix x3 = m 3 b32 + c32
12
1 1
I yy3 = m 3 a32 + c32 + m 3 a32
12 2
1 1
Izz3 = m 3 a32 + b32 + m 3 a32 .
12 2

and geometry of link 4 for y3 , x4
the robot A255
z3
x3 , z4
y3
z3
r3 x3
The kinematic and dynamic parameters of link 3 are shown in Table 12.3. The coor-
dinate frames and geometry of the fourth link is shown in Fig. 12.6.
The link is approximated by a solid disk and it is assumed that the center of mass
is located at the origin of frame ox4 y4 z4 . Therefore, the position vector of the center
of mass is given by
⎡ ⎤
c1 (a2 c2 + a3 c3 )
0
r̄ 4 = ⎣ s1 (a2 c2 + a3 c3 ) ⎦ (12.15)
d1 + a2 s2 + a3 s3
and the inertia tensor computed with respect to frame ox4 y4 z4 is given by
⎡1 ⎤
m 4 r42 0
4
0
I 4 = ⎣ 0 41 m 4 r42 0 ⎦ ,
0 0 21 m 4 r42
whose values can be seen in Table 12.4.

The last link is also approximated by a solid disk as shown in Fig. 12.7.
Table 12.4 Parameters of the links 4 and 5 for the robot A255
Link 4 Link 5
Parameter Value Units Parameter Value Units
r4 0.04 m r5 0.03 m
d4 0 m d5 0.0508 m
c4 0 m c5 0.045 m
m4 1 kg m5 0.5 kg

c5
the robot A255
y4 x5
z4
x4 z5
y5
x5
r5
z5
y5
The position of the center of mass of link 5 is then given by

⎡ ⎤
c1 (a2 c2 + a3 c3 + c5 c4 )
0
r̄ 5 = ⎣ s1 (a2 c2 + a3 c3 + c5 c4 ) ⎦ . (12.16)
d1 + a2 s2 + a3 s3 + c5 s5
On the other hand, the inertia tensor I 5 is given by

⎡1 ⎤
m 5r52 0
4
0
I 5 = ⎣ 0 41 m 5r52 0 ⎦ .
0 0 21 m 5r52
Finally, the kinematic and dynamic parameters of the link are given in Table 12.4.
With the previous information, the elements of the inertia matrix for the robot
A255 are given by
H11 = m 2 2c2 c2 2 + m 3 (a2 c2 + c3 c3 )2 + m 5 (a2 c2 + a3 c3 + c5 c4 )2

+ m 4 (a2 c2 + a3 c3 )2 + (Izz4 − Ixx4 ) s4 2 + Iyy1 + Iyy2 + I yy3 + Ixx4

+ Ixx5 c5 2 + Iyy5 s5 2 c4 2 + Izz5 s4 2 + Izz4 − Iyy5 + Izz5 s4 2

H12 = Ixx5 − Iyy5 c4 c5 s5

H13 = Ixx5 − Iyy5 c4 c5 s5

H14 = Ixx5 − Iyy5 c4 c5 s5
H15 =0
H22 = (m 3 + m 4 + m 5 ) a22 + m 2 2c2 + Izz2 + Izz3 + Iyy4 + Iyy5 c5 2
+ Ixx5 s5 2
H23 = (m 3 a2 c3 + (m 4 + m 5 )a2 a3 ) cos(q2 − q3 ) + Izz3 + Iyy4
+ Ixx5 s5 2 + Iyy5 c25
H24 = m 5 a2 c5 cos(q2 − q4 ) + Iyy4 + Ixx5 s5 2 + Iyy5 c5 2
H25 = 0
H33 = m 3 2c3 + (m 4 + m 5 )a32 + Iyy4 + Izz3 + Ixx5 s5 2 + Iyy5 c5 2
H34 = m 5 a3 c5 cos(q3 − q4 ) + Ixx5 s5 2 + Iyy5 c5 2 + Iyy4
H35 = 0

H44 = m 5 2c5 + Ixx5 − Iyy5 s5 2 + Iyy4 + Iyy
H45 = 0
H55 = Izz5 .
The matrix C(q, q) can be computed according to the procedure described in

Sect. 3.2 once the inertia matrix H(q) is known and it is omitted for simplicity. The
elements of the gravity vector g(q) ∈ R5 are given by
g1 = 0
5
g2 = c2 gc2 m 2 + a2 gc2 mj
j=3
5
g3 = c3 gc3 m 3 + a2 gc3 mj
j=4
g4 = c5 m 5 gc4
g5 = 0
Table 12.5 Main characteristic of the DC motors for the robot A255
Motor Brand Model Gear ratio
Motor 1 Cleveland motion M H 3515 r1 = 72
controls
controls
controls
controls
Motor 5 Tamagawa T S3253N r5 = 8
Table 12.6 Parameters of the DC motors for the robot A255

Motor Mechanical constant, Electrical constant, Armature resistance,
K a (Nm/A) K b (V rad/s) Ra ()
Motor 1 0.0657 0.0657 2.4
Motor 2 0.0657 0.0657 2.4
Motor 3 0.0657 0.0657 2.4
Motor 4 0.0053 0.0534 2.7
Motor 5 0.0053 0.0534 2.7
where g = 9.8 (m/s2 ) is the gravity constant. Finally the parameters of the different
DC motors of the robot are shown in Tables 12.5 and 12.6.
Chapter 13
Adapting the Robots CRS 465 and 255 for
Original Control Laws Implementation
Abstract The robots A465 and A255 by CRS Robotics are industrial manipulators
whose controllers are provided by the manufacturer and therefore they cannot be
programmed by the user. When it comes to the implementation of different control
laws or observers it is necessary to buy additional data acquisition boards if available
or, if these special boards are not available, then it is mandatory to adapt the devices
for this particular goal. This is not a minor challenge and this chapter makes a proposal
about how this can be done.
13.1 Original System
Figure 13.1 depicts a typical arrangement of an industrial robot and its control unit
as provided by the manufacturer.
This chapter introduces hardware level modifications of the CRS A465 and CRS
A255 robots (in what follows, A465 and A255 for short) and their corresponding
control modules, the CRS-C500C units (C500C for short), in order to have an open
architecture. Rather than a closed architecture, an open architecture allows to imple-
ment advanced control and estimation algorithms like those developed in Part II of
the book. Each industrial robot is actuated through an amplification stage with elec-
tromechanical protection mechanisms. The central control units process the signals
coming from the robots, i.e. joint encoders, which are employed in the default motion
control algorithm. The original system for the A255 and A465 industrial manipula-
tors is made up of the manipulators themselves along with a control unit (C500C)
and a teach pendant, as shown in Figs. 13.2 and 13.3, respectively. Some important
mechanical characteristics of the manipulators are summarized in Table 13.1.
The C500C unit has a 486-based embedded computer with an embedded oper-
ating system (CROS) developed by the manufacturer. This unit also has a power
supply stage for all the input and output devices, an input/output board to acquire the
encoder signals and to send the output voltages to the manipulator motors through
an amplification stage. The electrical characteristics of the amplification stage are
listed in Table 13.2.
https://doi.org/10.1007/978-3-030-85980-0_13
342 13 Adapting the Robots CRS 465 and 255 …
Fig. 13.1 Original communication of the industrial CRS manipulators with a PC
Fig. 13.2 Original

components of the CRS
A255 manipulator
Fig. 13.3 Original

components of the CRS
A465 manipulator
13.1 Original System 343
Table 13.1 Specifications of the CRS A465 and CRS A255 manipulators
Specification A465 robot A255 robot
Weight 32 kg 17 kg
Load (max) 3 kg 2 kg
Load (nominal) 2 kg 1 kg
Reach (X − Y ) 711.2 mm 560 mm
Actuators DC servo motors DC servo motors
Transmission Armonic gears Armonic gears and
straight/conic gears
Joint motion range
Waist +175◦ to −175◦ +175◦ to −175◦
Shoulder + 90◦ to −90◦ +110◦ to 0◦
Elbow +110◦ to −110◦ 0◦ to −125◦
Wrist 1 +180◦ to −180◦ –
Wrist 2 +105◦ to −105◦ +110◦ to -110◦
Wrist 3 +180◦ to −180◦ +180◦ to −180◦
Table 13.2 Electrical limit values at the power amplification stage: input voltage (vi ), output
voltage (vo ), and output current (i o )
Joint A465 robot A255 robot
vi (V) vo (V) i o (A) Gain vi (V) vo (V) i o (A) Gain
Waist ± 10 ± 70 12 7 ± 10 ± 25 2 2.5
Shoulder ± 10 ± 70 12 7 ± 10 ± 25 2 2.5
Elbow ± 10 ± 70 12 7 ± 10 ± 25 2 2.5
Wrist 1 ± 10 ± 30 3 3 – – – –
Wrist 2 ± 10 ± 30 3 3 ± 10 ± 25 2 2.5
Wrist 3 ± 10 ± 30 3 3 ± 10 ± 25 2 2.5
The joints are actuated by DC motors with different kinds of mechanical trans-
missions. The manufacturer controller consists of a closed-architecture PID posi-
tion/velocity control, which receives the encoder signals and computes the output
voltages. A diagram of this controller is shown in Fig. 13.4.
As mentioned above, the joint positions are measured by means of optical
encoders. The encoders provided in both the A255 and the A465 robots are of the
incremental type with two channels (A and B), i.e. are quadrature encoders. There is
a third channel that serves as a mark to calibrate the position measurement (channel
Z). A typical plot of these signals is shown in Fig. 13.5. The quadrature encoders
provided in both robots are the LDA-051-1000 by SUMTAK, powered by 5 V and
with a current consumption of 140 mA. Along with the three above channel signals,
the encoders also provide their logic negatives, i.e. the Ā, B̄, and Z̄ signals.
Fig. 13.4 Block diagram of the manufacturer’s controller
Fig. 13.5 Quadrature encoder signals
The C500C hardware is divided into three sections:

1. The upper section has the controller electronics
2. The lower one has the power stage
3. The emergency brake system and the signal acquisition stage.
The DC motor drivers are located in the remaining space at the side of the cabinet,
as can be seen in Fig. 13.6. In this figure, the back view of the C500C controller is
shown, where the connectors Robot Power and Robot Feedback can be distinguished.
The Robot Power connector is the motor power output, whereas the Robot Feedback
one has the encoders input signals.
The E-Stop is an emergency break button included in the original system as a part
of the C500C module as shown in Fig. 13.7.
When the E-Stop is pushed, it inhibits the power stage which drives the motors
of the A255 and A465 robots. They are blocked by means of electric relays, which
in turn must be liberated by a specific button for each robot. A copy of the E-Stop
button is located also in the Teach Pendant. A diagram of the emergency stop system
is displayed in Fig. 13.7. The connector SYSIO included in the C500C module allows
to control the signals for breaking/unbreaking. The ports 9, 10, 22, and 23 of this
connector where employed to implement a portable emergency button for each robot.
13.2 Hardware Modification 345
Fig. 13.6 Back view of the C500C controller
Fig. 13.7 Emergency break system of the C500C module
13.2 Hardware Modification
To open the controller architecture in order to program control algorithms different

from that of the manufacturer, the original PID controller must be inhibited. To carry
out this task, the encoder signals and the output voltages to the motors are in turn
processed by a personal computer (PC), and thus an acquisition stage is implemented
as follows. To process the input and output signals, an input/output acquisition unit,
in this case the CompactRIO module of National Instruments, is employed. The
CompactRIO has a programmable FPGA for processing high-demanding tasks. In
turn, the connection with the PC is made through fast-speed Ethernet communication.
The particular NI-cRIO 9014 model employed in this work has eight expansion
slots, which can be connected to one of the several I/O modules offered by National
Instruments.
In the present modification, two types of I/O modules were employed: the models
NI-9401 and NI-9263. The NI-9401 module is a general purpose digital input/output
board, with 8 configurable channels. This module supports TTL signals and has a
sampling rate of 9 MHz. The input voltages interval is 2–5.25 V. These modules are
employed to process the A, B, and Z encoder signals for a total of 11 joints, i.e. 6
and 5 for the A465 and the A255 robots, respectively. Thus, five NI-9401 modules
are employed in this adaptation.
In turn the NI-9263 can generate up to four analog signals by means of isolated
16-bits digital to analog converters (DAC). These channels have also built-in short-
circuit and over-voltage protectors. The output voltage limits are ±10 V, with load
currents up to 1 m A. The slew-rate for each channel is 4 V/μs. Since the total number
of joints for both manipulators is 11, three NI-9263 modules are employed for the
current modification.
The maximum electric power required by the CompactRIO and the eight afore-
mentioned modules is of 20 W. The source voltage must be between 19 and 30 V,
although the manufacturer recommends a power source of 24 V that supports at least
2 A. In order to satisfy these requirements, a 24 V Quint power source by Phoenix
Contact is employed, which is capable of delivering up to 5 A.
13.2.1 Signal Routing
Three DB25 connectors are employed to manage the signals coming to and from
the robots, namely [AB], [Z], and [VCOM], for each robot. The [AB] and [Z] wires
carry the digital signals coming from the encoders, while the [VCOM] send the
commanded voltages to the amplification stage which in turn sends the amplified
voltages to the motors. These wires are connected to the acquisition hardware by
an ad-hoc designed PCB board. The trace widths proposed for this board are of
24 mils (0.609 mm), supporting a maximum of 460 mA and a temperature rise of
1 ◦ C. A ground plane with a thickness of 35 mm is also included in the PCB, to avoid
crosstalk between adjacent traces. See Figs. 13.8 and 13.9.
13.2.2 Digital Stage for the A465 Manipulator
For each robot, the connections are divided into two parts, namely a digital and
an analog stage. The signals transmitted by the A465 manipulator encoders are the
digital channels A, B, and Z , and their digital negatives Ā, B̄, and Z̄ , respectively.
All these signals present a drop in the transmitted voltage as well as added noise due
to the imperfections on the transmission wires, as shown in Fig. 13.10.
Fig. 13.8 Upper view of the C500C module showing the added DB-25 [AB], [Z], and [VCOM]
connectors
As mentioned above, the NI-9401 modules employed for the acquisition of these
signals work with TTL signals, which is not the case for the unprocessed signals as
the one shown in Fig. 13.10. To solve this problem, the integrated circuit 26LS32 is
employed, which is a quadruple line receiver with three state outputs. The output of
this circuit is shown in Fig. 13.11, where it can be appreciated that the noise has been
significantly diminished.
Note that the number of signals is reduced to half after this circuit, since it com-
bines channels A, B, and Z with their logic negatives Ā, B̄, and Z̄ into one single
signal for each channel, which by abuse of notation will be called A, B, and Z . To
connect the 26LS32 output signals to the NI-9401 modules, an isolation stage is
proposed by means of HCPL2630 optocouplers. Each DIP package has two outputs
with a maximum speed of 10 Mb/s. For example, two channels for a joint encoder
can be connected to a single DIP as shown in Fig. 13.12.
R1 and R2 are set to 220 to have a polarization current for the internal led of
about 1.5 mA. The pull-up resistors R3 and R4 are set to 350 in order to minimize
the signal propagation time and the signal deformation. Finally, 15 pF capacitors are
included at the output side as recommended in the HCPL2630 datasheet.
Fig. 13.9 Back view of the C500C module showing the added DB-25 [AB], [Z], and [VCOM]
connectors
Fig. 13.10 Signal measured directly from channel A of the A465 first joint encoder
The HCPL2630 optocoupler output, which is connected to the NI-9401 module,

is shown in Fig. 13.13. The diagram containing all the encoder channels for the six
joints of the A465 manipulator is displayed in Figs. 13.14 and 13.15. Additionally to
the encoder signals, the robot A465 employs the H SW binary signals that indicates
the home position for each joint (see Fig. 13.16).
Fig. 13.11 Output of the 26LS32 line receiver for channel A of the A465 first joint
Fig. 13.12 Connection of the HCPL2630 optocouplers
Fig. 13.13 HCPL2630 optocoupler output for the channel A of the A465 first joint
Fig. 13.14 Signal conditioning circuit for the A465 manipulator encoders
13.2.3 Digital Stage for the A255 Manipulator
The A255 manipulator does not transmit neither the logic negative signals of the
encoders nor the H SW bits. Therefore, in this case the use of the 26LS32 line
receiver is omitted, and only the optocoupler stage is employed by means of the same
HCPL2630 DIP circuits mentioned before. The schematics of the circuit employed
Fig. 13.16 Signal conditioning circuit for the A465 manipulator H SW bits
Fig. 13.17 Signal measured directly from channel A of the A255 first joint encoder
Fig. 13.18 HCPL2630 output optocoupler for channel A of the A255 first joint
for this stage is displayed in Fig. 13.17. The output of the optocoupler is shown in
Fig. 13.18. The connection diagram of the incremental encoders for the robot A255
are depicted in Figs. 13.19 and 13.20.
13.2.4 Analog Stage
The NI-9263 modules have three stages:

1. A digital to analog (DAC) converter
2. A power amplifier
3. An over-voltage and short-circuit protection.
Therefore, no additional conditioning electronics is needed and the output of
the NI-9263 are connected directly to the DB25 [VCOM] connector that goes to
the C500C power amplifier. The connection of these three modules is shown in
Fig. 13.21 Analog stage wiring
Fig. 13.21. The modules share the same ground of the C500C module, but they are
isolated from the CompactRIO and the NI-9401 modules. Since each module has
four analog channels, i.e. a total of 12 analog channels are available, and the total
number of motors is 11, there is a remaining analog output channel. This channel is
employed to control the emergency brake, labeled as BT N _C T R L in Fig. 13.21.
13.2.5 Power Stage and Electric Protections
Figure 13.22 shows the final modified hardware for the main power source of the
CompactRIO and the designed electronic board delivers 24 V. To obtain a 5 V power
source, the Recom R785.0-0.5 integrated circuit is used. A 1N4004 diode is included
at the input to prevent undesirable backward currents. Also, a single-pole double-
throw relay is included, activated by two jumpers JP1 and JP2 (see Fig. 13.23).
13.3 Software Implementation
The encoder signals A, B, and Z of the 11 joint encoders and the output voltages
to the 11 motors are processed by the embedded system CompactRIO FPGA, which
is programmed according to the National Instruments software LabVIEW. Three
main components are employed in this program, and they are repeated for each joint.
13.3 Software Implementation 357
Fig. 13.22 Modified hardware of the C500C module
Table 13.3 Description of the virtual instruments
Therefore, three main virtual instruments are created to simplify the main program,
as shown in Table 13.3.
By taking into account the virtual instruments of Table 13.3, the encoder total
count and the output voltages generation for each joint is carried out according to the
flow diagram of Fig. 13.24. Next, this same flow diagram is repeated for all eleven
joints. Finally, the eleven blocks are included into a single Do-While cycle. A Tick
Count block was added to have control of the sample period, which is 5 μs. Therefore,
the obtained sample rate for the whole system is 200 kS/s.
Fig. 13.23 Protection circuits

13.3 Software Implementation 359
Fig. 13.24 Flow diagram of the encoder counter and the output voltage generator for one joint
Reference
CRS Robotics Corporation (2000) C500 controller user guide. CRS Robotics Corporation, Burling-
ton, Ontario, Canada
Chapter 14
The Geomagic Touch Haptic Device
Abstract The robots A465 and A255 by CRS Robotics are industrial manipulators
able to carry out a wide variety of tasks. However, for haptic purposes their inertias
can be too high, for which it is desirable to have available low inertia manipulators.
This is the case of he Geomagic Touch haptic device. This chapter develops its
kinematic and dynamic models.
14.1 Characteristics of the Geomagic Touch Manipulator
The Geomagic Touch haptic device, shown in Figs. 1.1 and 14.1, is an upgraded
version of the Phantom Omni device and it has become very popular in the last
few years since great number of researchers have employed it for the experimental
validation of different control algorithms. Such popularity is due to several factors,
in particular to its very high cost/benefit ratio. Additionally, its well developed API
makes it a very attractive choice for students and practitioners searching for a ready-
to-use platform. However, since this device is intended for haptics and virtual graphics
applications, there is not much information available about the kinematics from the
manufacturer. Moreover, despite its wide use, there are very few kinematic and
dynamic models available in the literature.
14.1.1 Kinematics of the Full Five DoF Robot
The kinematic model of the Geomagic Touch by considering all its five degrees
of freedom (DoF) is developed in this section. A Denavit-Hartenberg assignment
according to Algorithm 1 is shown in Fig. 14.2.
The propose of this assignment is to have the minimal number of parameters. For
this reason, the ox0 ,y0 ,z0 frame is located at the intersection of the first two joint axes.
There are some other practical considerations to be made, namely:
https://doi.org/10.1007/978-3-030-85980-0_14
362 14 The Geomagic Touch Haptic Device
Fig. 14.1 Geomagic Touch haptic device by 3D Systems
a2 d4 a5
z0 , y1 x3 , y2 x4 y5
y0 y4
x0 , x1 x2 , z3 x5
z1 z2 z4 z5
Fig. 14.2 Denavit-Hartenberg assignment for the 5 DoF Geomagic Touch configuration
• The rotation axes of joints 1 and 4, i.e. z 0 and z 3 are opposed to those defined by
the manufacturer. Therefore, the sign of the positions read from these joints must
be changed (multiplied by −1).
• As the rotation axis of the first joint is in the opposite direction, the torque com-
manded to the first joint must be also multiplied by −1. Since the 4th joint is not
actuated, this adjustment is not necessary for the fourth joint.
• The assignment of the x5 axis is also in the opposite direction. Therefore, an offset
of −180◦ must be added to the 5-th joint measurement.
• The actuator and the position sensor for the third joint are physically located at
the second joint. Thus, strictly speaking the Geomagic Touch robot is not a serial
manipulator. Nevertheless, it can be seen as a serial one, by making the adjustment
q3 = q3read − q2read , (14.1)

14.1 Characteristics of the Geomagic Touch Manipulator 363
Table 14.1 Adjustments of joint measurements

Joint Adjustment
1 q1 = −q1read
2 q2 = q2read
3 q3 = q3read − q2read
4 q4 = −q4read
5 q5 = q5read − π
Table 14.2 Denavit-Hartenberg parameters of the 5 DoF Geomagic Touch configuration

Joint ai [m] di [m] αi [rad] θi [rad]
1 0 0 π/2 θ1 ∗
2 0.145 0 0 θ2 ∗
3 0 0 π/2 θ3 ∗
4 0 0.138 −π/2 θ4 ∗
5 0.040 0 0 θ5 ∗
∗ variable quantity
where qiread , i = 1, . . . , 5, is the i-th joint value measured by employing the manu-
facturer API. A summary of the above adjustments is presented in Table 14.1.
14.1.2 Direct Kinematics of the Full Five DoF Robot
Given the above considerations, one can apply the Denavit-Hartenberg Algorithm 2
to obtain the parameters given in Table 14.2.
The values for a2 and d4 shown in Table 14.2 were obtained through a standard
kinematic calibration. Notice that the values employed by the manufacturer in the
API are a2 = 0.13335 [m] and d4 = 0.13335 [m], which are not physically accurate.
Now, it is possible to compute the homogeneous transformation from the ox0 y0 z0
frame to the ox5 y5 z5 frame, i.e. 0 T 5 = 0 R5 0 p5 . The end effector position is given
by
⎡ ⎤
a5 c5 (c1 c4 c23 + s1 s4 ) − a5 c1 s5 s23 + c1 (a2 c2 + d4 s23 )
0
p5 = ⎣ a5 c5 (c4 c23 s1 − c1 s4 ) − a5 s1 s5 s23 + s1 (a2 c2 + d4 s23 ) ⎦ ,
−c23 d4 + a2 s2 + a5 (c23 s5 + c4 c5 s23 )
whereas the components of the rotation matrix 0 R5 are

r11 = c5 s1 s4 + c1 (c4 c5 c23 − s5 s23 )

r12 = −s1 s4 s5 − c1 (c4 c23 s5 + c5 s23 )
r13 = c4 s1 − c1 c23 s4
r21 = c5 (c4 c23 s1 − c1 s4 ) − s1 s5 s23
r22 = (c1 s4 − c4 c23 s1 ) s5 − c5 s1 s23
r23 = −c1 c4 − c23 s1 s4
r31 = c23 s5 + c4 c5 s23
r32 = c5 c23 − c4 s5 s23
r33 = −s4 s23 .
There are two other coordinate frames that can be of interest, namely, the base and
the default frames. The base frame is the one located at the base of the robot, i.e. at the
intersection of the z 0 axis with the table that supports the manipulator. Accordingly,
this frame is just a translation over the z 0 axis of the ox0 y0 z0 frame in Fig. 14.2. On
the other hand, the default frame is the one defined by the manufacturer, related with
the ox0 y0 z0 frame in Fig. 14.2 by both a translation and a rotation. The homogeneous
transformations relating these two frames with the ox0 y0 z0 frame described above are
⎡ ⎤ ⎡ ⎤
1 0 0 0 0 0 1 tx
⎢ 0 1 0 0⎥ ⎢ 1 0 0 0 ⎥
Base
T0 = ⎢
⎣0
⎥ and Default
T0 = ⎢ ⎥,
0 1 d⎦ ⎣0 1 0 −tz ⎦
0 0 0 1 0 0 0 1
where d ≈ 0.17 [m], tx ≈ 0.1425 [m], and tz ≈ 0.0383 [m].
14.1.3 Differential Kinematics of the Full Five DoF Robot
As stated in Sect. 2.4, the manipulator geometric Jacobian J(q) can be decomposed
into two parts, namely
J v (q)
J(q) = , (14.2)
J ω (q)
where the components of the upper part are
jv11 = −a2 c2 s1 − d4 s23 s1 + a5 (−c4 c5 c23 s1 + s5 s23 s1 + c1 c5 s4 )

jv12 = a2 c1 c2 + a5 c1 c4 c5 c23 + a5 c5 s1 s4 + c1 (d4 − a5 s5 ) s23
jv13 = 0
jv21 = −c1 (a2 s2 + c23 (a5 s5 − d4 ) + a5 c4 c5 s23 )
jv22 = −s1 (a2 s2 + c23 (a5 s5 − d4 ) + a5 c4 c5 s23 )
jv23 = a2 c2 + a5 c4 c5 c23 + (d4 − a5 s5 ) s23
jv31 = c1 (c23 (d4 − a5 s5 ) − a5 c4 c5 s23 )
jv32 = s1 (c23 (d4 − a5 s5 ) − a5 c4 c5 s23 )
jv33 = a5 c4 c5 c23 + (d4 − a5 s5 ) s23
jv41 = a5 c5 (c4 s1 − c1 c23 s4 )
jv42 = −a5 c5 (c1 c4 + c23 s1 s4 )
jv43 = −a5 c5 s4 s23
jv51 = −a5 (s1 s4 s5 + c1 (c4 c23 s5 + c5 s23 ))
jv52 = −a5 ((c4 c23 s1 − c1 s4 ) s5 + c5 s1 s23 )
jv53 = a5 (c5 c23 − c4 s5 s23 ) ,
whereas the lower part is given by

⎡ ⎤
0 s1 s1 c1 s23 c4 s1 − c1 c23 s4
J ω (q) = ⎣ 0 −c1 −c1 s1 s23 −c1 c4 − c23 s1 s4 ⎦ .
1 0 0 −c23 −s4 s23 .
14.1.4 Dynamics of the Full Five DoF Robot
The dynamic model below was obtained by employing the Euler–Lagrange equations
of motion, as described in Sect. 3.1.1. These equations result in the well known
Lagrangian model
H(q)q̈ + C(q, q̇)q̇ + D q̇ + g(q) = τ . (14.3)
Since the matrix C(q, q̇) can be obtained from the matrix H(q) by computing the
Christoffel symbols of the first kind and D is a diagonal matrix accounting for the
viscous friction coefficients, only the H(q) matrix and the g(q) vector are given
below. The components of the matrix H(q) are given by
h 11 = m 2 2c2 c22 + m 3 (a2 c2 + s23 c3 )2 + m 4 (a2 c2 + s23 c4 )2
+ m 5 (a2 c1 c2 + c1 s23 (d4 − s5 c5 ) + c5 s1 s4 c5 + c1 c4 c5 c23 c5 )2
+ (a2 c2 s1 + (c4 c5 c23 s1 − c1 c5 s4 − s5 s23 s1 ) c5 + d4 s23 s1 )2
+ Ixx4 c42 s23

2
+ Ixx5 (c23 s5 + c4 c5 s23 )2 + Iyy5 (c5 c23 − c4 s5 s23 )2 + Iyy2 c22
+ Iyy4 c23
2
+ Izz3 c23
2
+ Ixx2 s22 + Ixx3 s23
2
+ Izz4 s42 s23
2
+ Izz5 s42 s23
2
+ Iyy1
h 12 = −m 5 c5 s4 c5 (a2 s2 + (c3 (c4 c5 s2 + c2 s5 ) + s3 (c2 c4 c5 − s2 s5 )) c5 − c23 d4 )
+ Ixx4 c4 s4 s23 + Ixx5 c5 s4 (c23 s5 + c4 c5 s23 ) − Iyy5 s4 s5 (c5 c23 − c4 s5 s23 )
− Izz4 c4 s4 s23 − Izz5 c4 s4 s23
h 13 = −m 5 c5 s4 c5 ((c3 (c4 c5 s2 + c2 s5 ) + s3 (c2 c4 c5 − s2 s5 )) c5 − c23 d4 )
+ Ixx4 c4 s4 s23 + Ixx5 c5 s4 (c23 s5 + c4 c5 s23 ) − Iyy5 s4 s5 (c5 c23 − c4 s5 s23 )
− Izz4 c4 s4 s23 − Izz5 c4 s4 s23
h 14 = −m 5 c5 c5 (a2 c2 c4 + (c2 (c3 c5 − c4 s3 s5 ) − s2 (c5 s3 + c3 c4 s5 )) c5 + c4 d4 s23 )
− Ixx5 s5 (c23 s5 + c4 c5 s23 ) − Iyy5 c5 (c5 c23 − c4 s5 s23 ) − Iyy4 c23
h 15 = m 5 s4 c5 (a2 c2 s5 + s23 (d4 s5 − c5 )) − s4 s23 Izz5
h 22 = m 2 2c2 + m 3 2a2 s3 c3 + a22 + 2c3 + m 4 2a2 s3 c4 + a22 + 2c4

+ m 5 (a2 s2 + c23 (s5 c5 − d4 ) + c4 c5 s23 c5 )2

+ (a2 c2 + c4 c5 c23 c5 + s23 (d4 − s5 c5 ))2 + Ixx5 c52 s42 + Izz4 c42 + Izz5 c42
+ Ixx4 s42 + Iyy5 s42 s52 + Iyy3 + Izz2

h 23 = m 3 c3 (a2 s3 + c3 ) + m 4 c4 (a2 s3 + c4 )
+ m 5 a2 ((c3 c4 c5 − s3 s5 ) c5 + d4 s3 ) + c42 c52 + s52 2c5 − 2d4 s5 c5 + d42
+ Izz4 c42 + Izz5 c42 + Ixx4 s42 + c52 Ixx5 s42 + Iyy5 s42 s52 + Iyy3
h 24 = m 5 c5 s4 c5 (−a2 s3 + s5 c5 − d4 ) − Ixx5 c5 s4 s5 + Iyy5 c5 s4 s5
h 25 = m 5 c5 (a2 (c3 c5 − c4 s3 s5 ) + c4 (c5 − d4 s5 )) + Izz5 c4
h 33 = m 3 2c3 + m 4 2c4 + m 5 c42 c52 + s52 2c5 − 2d4 s5 c5 + d42 + c52 s42 Ixx5
+ c42 Izz4 + c42 Izz5 + s42 Ixx4 + s42 s52 Iyy5 + Iyy3
h 34 = m 5 c5 s4 c5 (s5 c5 − d4 ) − Ixx5 c5 s4 s5 + Iyy5 c5 s4 s5
h 35 = m 5 c4 c5 (c5 − d4 s5 ) + Izz5 c4
h 44 = m 5 c52 2c5 + Iyy5 c52 + Ixx5 s52 + Iyy4
h 45 = 0
h 55 = m 5 2c5 + Izz5 .
The inertia tensors are computed by assuming that all the off-diagonal terms, i.e. the
cross products of inertia, are zero. The first body is a hollow sphere of radius r1 and
mass m 1 , with principal inertia moments given by
2
Ixx1 = Iyy1 = Izz1 = m 1r1.
3
The second and third bodies are considered solid cuboids, with sides b2 and c2 , and
b3 and c3 , respectively, and lengths a2 and l3 . While a2 is a Denavit-Hartenberg
parameter, l3 is the part of d4 that corresponds to the third link, so that when summed
to the part corresponding to the fourth link it satisfies d4 = l3 + l4 . The principal
moments of inertia for the second and third link, after applying the parallel axis
theorem, yields
m2 2
Ixx2 = b + c22
12 2
m2
Iyy2 = 4a22 + c22
12
m2
Izz2 = 4a22 + b22
12
m3
Ixx3 = 4l32 + c32
12
m3
Iyy3 = 4l32 + b32
12
m3 2
Izz3 = b + c32 .
12 3
The fourth and fifth bodies are assumed to be cylindrical, with radii r4 and r5 , and
lengths l4 and a5 , with l4 as explained above. For these two links, the principal
moments of inertia are
m4
Ixx4 =Izz4 = 3r42 + l42
12
m4 2
Iyy4 = r
2 4
m5
Iyy5 =Izz5 = 3r52 + a52 + m 5 δ52
12
m5 2
Ixx5 = r ,
2 5
where δ5 is the distance from the center of mass of the fifth link to the z 5 axis. On
the other hand, the components of the vector g(q) are computed as
g1 = 0

g2 = g c2 c2 m 2 + c4 c5 c23 c5 m 5 + a2 c2 (m 3 + m 4 + m 5 )

+ c3 m 3 s23 + c4 m 4 s23 + d4 m 5 s23 − c5 m 5 s5 s23
g3 = g (c3 m 3 s23 + c4 m 4 s23 + m 5 (d4 s23 + c5 (c4 c5 c23 − s5 s23 )))
g4 = −gc5 c5 m 5 s4 s23
g5 = gc5 m 5 (c5 c23 − c4 s5 s23 ) ,
where g is the acceleration of gravity constant.
14.2 Simplified Three DoF Geomagic Touch
A widely used configuration for the Geomagic Touch consists in a simplified fully-
actuated three DoF robot by blocking the last two joints. As shown in Fig. 14.3, the
last link is not straight because the fifth joint cannot be fixed at −90◦ .
14.2.1 Kinematics of the Three DoF Robot
The Denavit-Hartenberg assignment for this configuration is shown in Fig. 14.4.

The ox0 ,y0 ,z0 frame is located at the intersection of the first two joint axes. For this
configuration, some practical considerations are:
• The rotation axis of joint 1, i.e. z 0 , is opposed to that considered by the manufac-
turer. Therefore, the sign of the position read from this joint must be changed (mul-
tiplied by −1).
• The torque commanded to the first joint must be also multiplied by −1.
• The actuator and the position sensor for the third joint are physically located at
the second joint. Thus, strictly speaking the Geomagic Touch robot is not a serial
Fig. 14.3 Mechanical

blocking of the last three
joints of the Touch robot
14.2 Simplified Three DoF Geomagic Touch 369
a2 a3
z0 , y1 x3 , y2 y3
y0
x3
x0 , x1 x2 , z3
z1 z2
z3
Fig. 14.4 Denavit-Hartenberg assignment for the 3 DoF configuration
Table 14.3 Adjustments of Joint Adjustment

joint measurements
1 q1 = −q1read
2 q2 = q2read
3 q3 = q3read − q2read
manipulator. Nevertheless, it can be seen as a serial one, by making the adjustment
q3 = q3read − q2read , (14.4)
where qiread , i = 1, . . . , 3, is the i-th joint value measured by employing the man-
ufacturer API.
A summary of the above adjustments is presented in Table 14.3.
14.2.2 Direct Kinematics of the Three DoF Robot
By applying the Denavit-Hartenberg Algorithm 2 one obtains the parameters given

in Table 14.4.
The distance a3 is computed by means of the law of cosines as

a3 = 0.1382 + 0.042 − 2(0.138)(0.04) cos(150∗ )
The homogeneous
transformation
from the ox0 y0 z0 frame to the ox3 y3 z3 frame is denoted
by 0 T 3 = 0 R3 0 p3 , from which the end effector position is given by
Table 14.4 Denavit-Hartenberg parameters of the 3 DoF configuration

Joint ai [m] di [m] αi [rad] θi [rad]
1 0 0 π/2 θ1 ∗
2 0.145 0 0 θ2 ∗
3 0.1738 0 0 θ3 ∗
∗ variable quantity
⎡ ⎤
(a2 c2 + a3 c23 ) c1
0
p3 = ⎣ (a2 c2 + a3 c23 ) s1 ⎦ ,
a2 s2 + a3 s23
whereas the rotation matrix 0 R3 is computed to be

⎡ ⎤
c1 c23 −c1 s23 s1
0
R3 = ⎣ c23 s1 −s1 s23 −c1 ⎦ .
s23 c23 0
The same coordinate frames transformations Base T0 and Default T0 of Sect. 14.1.2
can be employed for this case to find the end-effector coordinates with respect to the
base and default frames.
14.2.3 Differential Kinematics of the Three DoF Robot
As stated in Sect. 2.4, the manipulator geometric Jacobian J(q) can be decomposed
into two parts, namely
J v (q)
J(q) = , (14.5)
J ω (q)
where the upper part is

⎡ ⎤
− (a2 c2 + a3 c23 ) s1 −c1 (a2 s2 + a3 s23 ) −a3 c1 s23
J v (q) = ⎣ c1 (a2 c2 + a3 c23 ) −s1 (a2 s2 + a3 s23 ) −a3 s1 s23 ⎦ ,
0 a2 c2 + a3 c23 a3 c23
and the lower part is given by

⎡ ⎤
0 s1 s1
J ω (q) = ⎣ 0 −c1 −c1 ⎦ .
1 0 0
14.2.4 Dynamics of the Three DoF Robot
The corresponding Lagrangian model, similar to that given in Sect. 14.1.4 is devel-
oped below for this configuration. Since for the present case the dynamic model is
much shorter, along with the matrix H(q) and the vector g(q), the matrix C(q, q̇)
is given as well, computed by means of the Christoffel symbols of the first kind. The
components of the matrix H(q) are
h 11 = m 2 c22 2c2 + m 3 (a2 c2 + c23 c3 ) 2 + c22 Iyy2 + c23

2
Iyy3 + s22 Ixx2 + s23
2
Ixx3 + Iyy1
h 12 = 0
h 13 = 0
h 22 = m 2 2c2 + m 3 2a2 c3 c3 + a22 + 2c3 + Izz2 + Izz3
h 23 = m 3 c3 (a2 c3 + c3 ) + Izz3
h 33 = m 3 2c3 + Izz3 .
The inertia tensors are computed by assuming that all the off-diagonal terms, i.e. the
cross products of inertia, are zero. The first body is a hollow sphere of radius r1 and
mass m 1 , with principal inertia moments given by
2
Ixx1 = Iyy1 = Izz1 = m 1r12 .
3
The second and third bodies are considered as solid cuboids, with sides b2 and h 2 ,
and b3 and h 3 , respectively, and lengths a2 and a3 . The principal moments of inertia
for the second and third link, after applying the parallel axis theorem, yields
m2
Ixx2 = b22 + h 22
12
m2
Iyy2 = 4a22 + h 22
12
m2
Izz2 = 4a22 + b22
12
m3
Ixx3 = b32 + h 23
12
m3
Iyy3 = 4a32 + h 23
12
m3
Izz3 = 4a32 + b32 .
12
The components of the 3 × 3 matrix C(q, q̇) are given by1
1 The notation ceij is used to avoid confusion with the shorthand notation for the cosine function.
ce11 = c2 q̇2 s2 Ixx2 + c23 q̇2 s23 Ixx3 + c23 q̇3 s23 Ixx3 − c2 q̇2 s2 Iyy2 − c23 q̇2 s23 Iyy3

− c23 q̇3 s23 Iyy3 − m 2 c2 q̇2 s2 2c2 + m 3 − a2 c23 q̇2 s2 c3 − a2 c2 q̇2 s23 c3

− a2 c2 q̇3 s23 c3 − a22 c2 q̇2 s2 − c23 q̇2 s23 2c3 − c23 q̇3 s23 2c3
ce12 = c2 q̇1 s2 Ixx2 + c23 q̇1 s23 Ixx3 − c2 q̇1 s2 Iyy2 − c23 q̇1 s23 Iyy3 − c2 q̇1 m 2 s2 2c2

+ m 3 − 2a2 c2 c3 q̇1 s2 c3 − a2 c22 q̇1 s3 c3 − a22 c2 q̇1 s2 + a2 q̇1 s22 s3 c3 + c2 q̇1 s2 s32 2c3

− c2 c32 q̇1 s2 2c3 − c22 c3 q̇1 s3 2c3 + c3 q̇1 s22 s3 2c3

ce13 = c23 q̇1 s23 Ixx3 − c23 q̇1 s23 Iyy3 + m 3 −a2 c2 q̇1 s23 c3 − c23 q̇1 s23 2c3
ce21 = −c2 q̇1 s2 Ixx2 − c23 q̇1 s23 Ixx3 + c2 q̇1 s2 Iyy2 + c23 q̇1 s23 Iyy3 + m 2 c2 q̇1 s2 2c2

+ m 3 2a2 c2 c3 q̇1 s2 c3 + a2 c22 q̇1 s3 c3 + a22 c2 q̇1 s2 − a2 q̇1 s22 s3 c3 − c2 q̇1 s2 s32 2c3

+ c2 c32 q̇1 s2 2c3 + c22 c3 q̇1 s3 2c3 − c3 q̇1 s22 s3 2c3
ce22 = −m 3 a2 q̇3 s3 c3

ce23 = −m 3 (a2 q̇2 s3 c3 + a2 q̇3 s3 c3 )
ce31 = c23 q̇1 s23 Iyy3 − c23 q̇1 s23 Ixx3 + m 3 q̇1 s23 c3 (a2 c2 + c23 c3 )
ce32 = m 3 a2 q̇2 s3 c3

ce33 = 0.
Finally, the vector g(q) can be computed to be

⎡ ⎤
0
g(q) = ⎣ gc2 c2 m 2 + g (a2 c2 + c23 c3 ) m 3 ⎦ .
gc23 c3 m 3
14.2.5 Linear Parametrization of the Three DoF Robot
A linear parametrization and a model identification of the three DoF robot is carried
out in this section in order to identify the constant quantities in the dynamic model.
In order to obtain a more compact model, in this section it is assumed that the second
and third bodies are slender rods instead of solid cuboids, as considered before. This
simplification implies that the distances r1 , b2 , h 2 , b3 , and h 3 can be assumed to
be zero and in consequence Iyy1 = Ixx2 = Ixx3 = 0, and Iyy2 = Izz2 I2 and Iyy3 =
Izz3 I3 . Recall, from Property 3.14 that the left hand side of the Lagrangian model
can be expressed as a product of a regressor Y (q, q̇, q̈) ∈ Rn× p and a vector of
constant parameters ϕ ∈ R p , where p is the number of parameters, i.e.
H(q)q̈ + C(q, q̇)q̇ + D q̇ + g(q) = Y (q, q̇, q̈)ϕ.
For the present case, a possible vector of constant parameters is

⎤ ⎡
⎡ ⎤
π1 m 2 c22 2c2 + m 3 a22 + I2
⎢π2 ⎥ ⎢ m 3 a2 c3 ⎥
⎢ ⎥ ⎢ ⎥
⎢π3 ⎥ ⎢ m 3 c3 + I3
2 ⎥
⎢ ⎥ ⎢ ⎥
⎢π4 ⎥ ⎢ cf1 ⎥
ϕ=⎢ ⎥=⎢
⎢π5 ⎥ ⎢
⎥,
⎥ (14.6)
⎢ ⎥ ⎢ cf2 ⎥
⎢π6 ⎥ ⎢ cf3 ⎥
⎢ ⎥ ⎢ ⎥
⎣π7 ⎦ ⎣ m 2 c2 ⎦
π8 m 3 c3
and the components of the regressor are given by
y11 = c22 q̈1 − 2c2 s2 q̇1 q̇2

y12 = c2 c23 q̈1 − (c2 s23 (q̇2 + q̇3 ) + s2 c23 q̇2 )q̇1
y13 = s23
2
q̈1 + c23 s23 (q̇2 + 2q̇3 )q̇1 + s23 c23 q̇1 q̇2
y14 = q̇1
y15 = y16 = y17 = y18 = 0
y21 = q̈2 + c2 s2 q̇12
1
y22 = 2c3 q̈2 + c3 q̈3 + (s2 c23 + c2 s23 )q̇12 − 2s3 q̇2 q̇3 − s3 q̇32
2
y23 = q̈2 + q̈3 − s23 c23 q̇12
y24 = 0
y25 = q̇2
y26 = 0
y27 = gc2
y28 = gc23
y31 = 0
1
y32 = c2 q̈2 + c2 s23 q̇12 + s3 q̇22
2
y33 = q̈2 + q̈3 − c23 s23 q̇12
y34 = y35 = 0
y36 = q̇3
y37 = 0
y38 = gc23 .
Table 14.5 Parameters estimation for the 3 DOF configuration

Parameter Meaning Estimated value
π1 m 2 c22 2c2 + m 3 a22 + I2 0.00453641
π2 m 3 a2 c3 0.000341864
π3 m 3 2c3 + I3 0.004503919
π4 cf1 0.001373760
π5 cf2 0.001188021
π6 cf3 0.000553657
π7 m 2 c2 0.004948273
π8 m 3 c3 0.012955526
The above parameters were experimentally obtained by a standard least squares

off-line algorithm and the results are shown in Table 14.5.
In terms of the parameters in Table 14.5, the inertia matrix H(q) and the gravity
vector g(q) are given by:
⎡ ⎤
c22 π1 + c2 c23 π2 + s23
2
π3 0 0
H(q) = ⎣ 0 π1 + 2c3 π2 + π3 c3 π2 + π3 ⎦ (14.7)
0 c3 π2 + π3 π3
⎡ ⎤
0
g(q) = ⎣g (c2 π7 + c23 π8 )⎦ , (14.8)
gc23 π8
whereas the components of the C(q, q̇) matrix are
1
ce11 = −c2 s2 q̇2 π1 − (c2 s23 (q̇2 + q̇3 ) + s2 c23 q̇2 ) π2 + c23 s23 (q̇2 + q̇3 )π3
2
1
ce12 = −c2 s2 q̇1 π1 − (s2 c23 + c2 s23 ) q̇1 π2 + s23 c23 q̇1 π3
2
1
ce13 = − c2 s23 q̇1 π2 + c23 s23 q̇1 π3
2
1
ce21 = c2 s2 q̇1 π1 + (s2 c23 + c2 s23 ) q̇1 π2 − s23 c23 q̇1 π3
2
ce22 = −s3 q̇3 π2
ce23 = −s3 (q̇2 + q̇3 ) π2
1
ce31 = c2 s23 q̇1 π2 − c23 s23 q̇1 π3
2
ce32 = s3 q̇2 π2
ce33 = 0.

ArteagaMA Local Stability Springer

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

ArteagaMA Local Stability Springer

Uploaded by

Copyright:

Available Formats

Lecture Notes in Electrical Engineering 798

More information about this series at https://link.springer.com/bookseries/7818

Local Stability and Ultimate

ISSN 1876-1100 ISSN 1876-1119 (electronic)

There is a wide variety of applications for industrial robot manipulators, such as

Mexico City, Mexico Marco A. Arteaga

2.3.2 Kinematic Decoupling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

Part II Looking for Semiglobal Stability or Ultimate Boundedness

Part III Different Testbeds and the Adaptation of Industrial

1.1 Brief History of Robot Manipulators

1.2 Industrial Robots

Definition 1 An industrial robot is an automatically controlled, reprogrammable,

Fig. 1.2 Revolute and Revolute Prismatic

1.3 Common Kinematic Arrangements

1.3.1 Articulated Manipulator

This arrangement is also known as revolute or anthropomorphic. The three robots

1.3.2 Spherical Manipulator

Fig. 1.3 Articulated elbow z0

side view top view

Fig. 1.4 Workspace of an articulated elbow manipulator

Fig. 1.5 Spherical z0

1.3.3 SCARA Manipulator

Fig. 1.6 Workspace of a

Fig. 1.7 SCARA z0 z1 z2

1.3.4 Cylindrical Manipulator

Fig. 1.8 Workspace of a

Fig. 1.9 Cylindrical d3

Fig. 1.10 Workspace of the

1.3.5 Cartesian Manipulator

Fig. 1.11 Cartesian d2

Fig. 1.12 Workspace for a

1.4 Wrists and End-Effectors

1.4.1 Spherical Wrist

Fig. 1.13 Spherical wrist

Fig. 1.14 Gripper as

1.4.2 Common End-Effectors

1.5 Some Other Important Issues to Take into Account

Abstract In order to assign a task to a robot manipulator, it is a key matter to describe

2.1 Rigid Motions and Homogeneous Transformations

p = px0 i0 + p y0 j 0 + pz0 k0 . (2.1)

© The Author(s), under exclusive license to Springer Nature Switzerland AG 2022 15

Fig. 2.1 Representation of a z0

Recall that the dot product is commutative.

p = px1 i1 + p y1 j 1 + pz1 k1 , (2.6)

Fig. 2.2 Representation of a z0

p = px0 i0 + p y0 j 0 + pz0 k0 = px1 i1 + p y1 j 1 + pz1 k1 . (2.9)

Fig. 2.3 Representing p

px0 = p · i0 = px1 i1 · i0 + p y1 j 1 · i0 + pz1 k1 · i0 . (2.10)

In the very same fashion it is possible to obtain

p y0 = px1 i1 · j 0 + p y1 j 1 · j0 + pz1 k1 · j 0 (2.11)

The last relationships can be written in compact form

Quite obviously, 1 R0 is the inverse transformation of 0 R1 . By taking into account

Understanding the meaning of the columns of 0 R1 actually allows to compute it in a

Keep in mind that the regular definition of i, j and k, i.e.

Fig. 2.4 Basic rotation y0

This way, the rotation matrix for this example is given by

Note that when θ = 0◦ then it holds

Rz,θ+φ = Rz,θ Rz,φ . (2.25)

Combining (2.24) with (2.25) yields to the conclusion that

2.1.2 Composition of Rotations

Recall that the rotation matrix 0 R1 in (2.13):

that the following relationships hold

Therefore, from (2.31) and (2.33) one concludes that

Fig. 2.5 Rotations with respect a fixed frame

where ω = θ̇ j is the angular velocity over the y axis.