# Integration of vision and force control for physical interaction with robotic manipulators

Alexander Oliva

<figure><img src="image_1.png" /></figure>

## To cite this version:

Alexander Oliva. Integration of vision and force control for physical interaction with robotic manipulators. Robotics [cs.RO]. Université Rennes 1, 2022. English. NNT: 2022REN1S007. tel-03690426v2

HAL Id: tel-03690426

https://theses.hal.science/tel-03690426v2

Submitted on 10 Jun 2022

HAL is a multi-disciplinary open access archive for the deposit and dissemination of scientific research documents, whether they are published or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d'enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# THÈSE DE DOCTORAT DE

ÉCOLE DOCTORALE 601

Mathématiques et Sciences et Technologies
de l'Information et de la Communication

Spécialité : Automatique, Productique et Robotique

Par

Alexander OLIVA

## Integration of vision and force control for physical interaction with robotic manipulators

Thèse présentée et soutenue à Rennes, le 22/02/2022

Unité de recherche : Inria-Rennes/IRISA

## Rapporteurs avant soutenance :

Andrea CHERUBINI Professeur, Université de Montpellier, LIRMM

Youcef MEZOUAR Professeur, Université Clermont-Auvergne, Sigma

## Composition du Jury :

Président : Véronique PERDEREAU

Examineurs : Joris DE SCHUTTER

Andrea CHERUBINI

Youcef MEZOUAR

Véronique PERDEREAU

Dir. de thèse : François CHAUMETTE

Co-dir. de thèse : Paolo ROBUFFO GIORDANO

Professeure, Sorbonne Université, Paris, ISIR, France

Professeur, KU Leuven, Leuven (Arenberg), RAM, Belgique

Professeur, Université de Montpellier, LIRMM, France

Professeur, Université Clermont-Auvergne, Sigma, France

Professeure, Sorbonne Université, Paris, ISIR, France

Directeur de recherche Inria, Inria/IRISA Rennes, France

Directeur de recherche CNRS,IRISA/Inria Rennes, France
# Résumé en français

## Introduction

Robotique, c'est le terme qu'en 1941 Isaac Asimov a pris dans sa nouvelle "Liar!", pour désigner la science et la technologie étudiant les robots, les machines mécaniques qu'il imaginait dépourvues de sentiments et dotées d'un cerveau "positronic" programmé par l'homme. Le terme robot, quant à lui, est apparu pour la première fois, vingt ans plus tôt (1920) dans le dramma "R.U.R." (Rossum's Universal Robots) de l'écrivain tchèque Karel Čapek dans lequel des organismes biologiques artificiels, pouvant être confondus avec des humains, avaient pour mission de remplacer les êtres humains dans des emplois subalternes; Ce mot, inventé par le frère de Karel Čapek Josef, vient du tchèque "robota" qui signifie en effet "travail dur" ou "travail forcé" et est dérivé de rab, qui signifie "esclave". Même si ce terme "moderne" date d'un siècle, notre désir de construire des machines capables de remplacer l'homme dans n'importe quelle situation disparate ou dans des tâches fastidieuses et/ou répétitives, ou encore dangereuses, a des racines bien plus anciennes, aujourd'hui plus ferventes que jamais (voir Figure 1).

Les usines modernes font largement appel à des manipulateurs robotisés pour effectuer des tâches très spécifiques, restreintes et déterministes, i.e., le soudage, le pick-and-place ou la pulvérisation. (comme le montre la figure 2), pour lesquelles la séquence exacte des opérations à effectuer sur la chaîne de montage est parfaitement connue a priori. Avec ces restrictions et en considérant les environnements hautement structurés et bien contrôlés dans lesquels ces robots sont couramment utilisés, la conception du commande de la tâche devient relativement simple en recourant à des techniques de planification de chemin/trajectoire, qui bénéficieront des excellentes capacités cinématiques des robots industriels et de la connaissance préalablement disponible de l'environnement.

Ces robots ont été utilisés comme de simples automates polyvalents, pour automatiser les processus de production industrielle sous la justification d'une plus grande productivité et d'un meilleur contrôle de la qualité, sans vraiment exploiter leur formidable flexibilité par rapport aux machines automatisées fixes. Cela
<figure><img src="image_3.png" /><figcaption>**Figure 1** – De la mythologie grecque à aujourd'hui : (a) Talos ("TAΛΩΝ") était un automate géant fait de bronze pour protéger Europe en Crète (~ 700 BC). (b) Le "Chevalier Automaton", était un automate humanoïde conçu par Leonardo da Vinci vers 1495. (c) Unimate : c'est le premier robot industriel, inventé par George Devol et mis en service en 1961. (d) Atlas : la nouvelle génération. Développé par Boston Dynamics en 2016, est probablement le robot le plus avancé jamais construit à ce jour.</figcaption></figure>

est également dû au fait que les manipulateurs industriels, à la différence des robots universels de Rossum, sont peu sophistiqués et simples d'esprit, avec de très faibles capacités de prise de décision dès leur sortie de l'usine (les robots industriels commerciaux sont généralement équipés de capteurs de position articulaire uniquement). La flexibilité est-elle en effet la véritable force qui distingue man, le manipulateur le plus polyvalent de la nature, par opposition aux machines. Cette grande flexibilité découle en partie de la capacité à traiter des informations sensorielles multiples, d'où notre intérêt à projeter ces capacités sensorielles dans les robots industriels. Cela permettra même aux petites entreprises ayant des volumes de production limités et des produits différents de bénéficier de cette technologie. La possibilité d'un tel scénario avait déjà été envisagée par Bajcsy [1] il y a quatre décennies, et l'essor des Cobots, ou robots collaboratifs, est en effet l'une des technologies clés de la révolution industrielle moderne, également connue sous le nom d'industrie 4.0, qui pousse sur le front de la smart production, c'est-à-dire
<figure><img src="image_4.png" /><figcaption>**Figure 2** – (a) Ligne d'assemblage de la giga-usine Tesla : à chaque poste, plusieurs manipulateurs équipés de soudeuses à l'arc appliquent les points de soudure préprogrammés. (b) Trajet planifié pour une opération automatisée de pulvérisation sur la carrosserie d'une voiture. (c) Un robot SCARA effectuant des opérations de pick-and-place dans un environnement bien structuré.</figcaption></figure>

dire le front des nouvelles technologies de production qui créent une collaboration entre tous les éléments présents dans le processus de production, c'est-à-dire une collaboration entre l'opérateur, les machines et les outils.

Bien que les approches de planification de trajectoire aient prouvé leur efficacité dans des applications bien structurées, ce qui, entre autres, nécessite un effort considérable de modélisation et de calibration, ces méthodes sont difficilement applicables aux scénarios hautement dynamiques illustrés ci-dessus, qui pourraient même impliquer des contacts volontaires ou accidentels avec un opérateur humain ou l'environnement. En revanche, la commande basé sur les capteurs offre une plus grande souplesse dans la gestion des incertitudes qui caractérisent les environnements non structurés/dynamiques ou même non rigides.

Parmi toutes les modalités de détection disponibles, la vision et la détection de force sont parmi les plus complémentaires et relativement peu coûteuses. Les caméras sont capables de fournir une description riche de la scène dans laquelle le manipulateur se déplace, ce qui permet, par exemple, d'aligner avec précision l'effecteur final avec un objet dans un environnement imprécisément calibré ou changeant dynamiquement. En revanche, les capteurs de force fournissent des informations beaucoup plus précises, bien que localisées, qui permettent d'effectuer la tâche de commande en question avec une plus grande précision qu'en utilisant
uniquement la vision.

Avant même de penser à l'intégration multi-sensorielle, il faut d'abord définir et caractériser les propriétés de commande de chaque modalité sensorielle individuellement. Les premières recherches sur la vision et la détection de la force se sont concentrées sur la compréhension des point fort et des faiblesses des stratégies de commande liées à chacun de ces capteurs séparément. En fait, deux communautés fortes, celle de la commande de la force et celle de l'asservissement visuel, ont produit un certain nombre de résultats importants au cours des dernières décennies, amenant leurs domaines respectifs à un état de maturité assez avancé.

Cette thèse se situe à mi-chemin entre ces deux mondes et vise à apporter des réponses au problème difficile du couplage de la vision et de détection de force en proposant à la fois des stratégies de commande combinées innovantes et les outils nécessaires pour soutenir l'étude, par example en simulation, et la mise en œuvre de ces stratégies. Le besoin de développer de nouvelles techniques de commande intégré vision-force qui soient plus sûres, plus robustes et plus performantes est également motivé par l'adoption croissante de robots dans les petites et moyennes entreprises, où les robots se retrouvent à partager l'espace de travail avec des opérateurs humains ou d'autres machines ; sans surprise, les modalités de vision et de détection de force s'avèrent être les configurations sensorielles les plus communément adoptées dans le contexte de l'interaction physique homme-robot (pHRI).

## Les difficultés

Les approches intégrées de la vision et de la force suscitent bien sûr un grand intérêt, car le développement d'applications combinées de la vision et de la force n'est qu'une étape naturelle dans l'évolution de la robotique basée sur les capteurs [2], permettant de combiner les avantages des deux capteurs et de surmonter les faiblesses individuelles. Cependant, le processus d'intégration fait naître des besoins supplémentaires, et il n'est pas simple de parvenir à une utilisation combinée efficace de la vision et de la force. Les capteurs de vision et de force mesurent des quantités fondamentalement différentes, ce qui entraîne des difficultés inhérentes lorsqu'on essaie de combiner les données des deux capteurs. Les approches d'intégration de capteurs nécessitent une représentation commune de l'entrée sensorielle à intégrer, mais les capteurs de force et de vision ne partagent pas une telle représentation commune des données qu'ils fournissent. De plus, en raison de leur nature distincte, les deux capteurs ne sont souvent utiles qu'à des étapes distinctes de la tâche à accomplir. D'autre part, les schémas de commande
de la force les plus courants utilisent des capteurs de force/couple montés sur le poignet, qui sont capables de mesurer n'importe quel type de force, qu'il s'agisse de forces gravitationnelles, inertielles ou de contact. Le couplage inertiel de la masse de l'effecteur (et éventuellement de la charge utile) positionnée en aval du capteur introduit un problème fondamental lorsque les directions contrôlées sont partagées entre la vision et la détection de la force. D'autres aspects caractérisant les deux capteurs sont les taux auxquels ils peuvent fournir des échantillons. Les capteurs de force/couple fournissent généralement des mesures à très haute fréquence (1 kHz) qui nécessitent peu de traitement ou d'interprétation pour être utilisées dans une boucle de commande. Les capteurs de vision, quant à eux, fonctionnent à des vitesses beaucoup plus lentes (typiquement 30 à 60 fps) et leurs mesures nécessitent des niveaux élevés de traitement pour être utilisables dans un algorithme de commande, ce qui introduit une latence non négligeable dans la disponibilité de la quantité de commande. Si l'on considère que la vitesse d'asservissement d'un manipulateur doit être au moins dix fois supérieure à sa fréquence de résonance mécanique [3], la vitesse de retour limitée dans les systèmes de vision s'avère être le principal problème limitant l'applicabilité de la commande visuelle aux tâches robotiques réelles et son intégration dans les systèmes de retour de capteurs conventionnels [4]. Enfin, la simplification des modèles utilisés pour décrire le système, comme la négligence de certains phénomènes difficiles à modéliser, par exemple la friction, peut en pratique avoir des effets significatifs sur le comportement du système.

Dans cette thèse, nous abordons plusieurs de ces défis tels que :

* l'intégration sensorielle, vue du point de vue du commande,

* l'augmentation du taux d'échantillonnage de l'information visuelle,

* l'atténuation des effets négatifs liés à la latence des données, et

* la modélisation du système et l'identification de ses paramètres.

## Contributions

Dans cette thèse, nous avons contribué à la synthèse de contrôleurs qui réalisent l'intégration sensorielle de la vision et de la force directement dans l'espace des capteurs, c'est-à-dire dans l'espace des caractéristiques visuelles. Ces contrôleurs atteignent le plus haut degré d'intégration sensorielle, car ils permettent de contrôler toutes les directions possibles avec les deux modalités sensorielles.
En particulier, l'une des premières contributions de ce travail a été le mappage
des forces physiques agissant sur le manipulateur dans l'espace des caractéristiques
visuelles. Cela a permis de dériver, d'une part, un commande d'impédance dans
l'espace des caractéristiques visuelles et, d'autre part, d'étendre le concept de
repere conforme à l'espace des caractéristiques. Le premier est un contrôleur à dy-
namique inverse, qui permet de prendre en compte la dynamique du manipulateur
dans la synthèse du contrôleur qui ne nécessite pas, dans sa version la plus simple,
de capteur de force/couple pour fonctionner. Grâce au mappage des forces dans
l'espace des caractéristiques visuelles, il a été possible de définir une loi d'admit-
tance dans cet espace, que nous avons utilisée pour étendre les capacités de l'un
des schémas de commande vision/force de la littérature.

Un autre aspect que nous avons abordé est la réduction de l'écart de taux
entre les capteurs. L'utilisation d'un cadre d'estimation bayésien nous a permis
d'augmenter virtuellement la disponibilité des données visuelles à un taux plus
élevé et également d'atténuer les effets négatifs dus à la latence des algorithmes de
vision par ordinateur. Les modèles d'estimation proposés ont également permis de
reconstruire la vitesse d'une cible mobile observée par la caméra. Ces estimateurs
peuvent être facilement utilisés dans différents schémas de commande.

Les différents schémas de commande développés dans ce travail ont été vali-
dés par des simulations et des expériences approfondies sur une plateforme réelle
dans notre laboratoire. En ce qui concerne les simulations, nous avons également
développé dans ce travail un environnement de co-simulation physique qui permet
non seulement de prototyper rapidement des applications d'asservissement visuel,
mais aussi de réaliser des simulations dynamiques avec interaction physique avec
l'environnement. Grâce à ce simulateur, il a été possible d'étudier pas à pas tous les
problèmes et limitations des contrôleurs d'asservissement visuel de second ordre
(a.k.a. Dynamic Visual Servoing). Nous avons également pu concevoir et tester les
estimateurs qui nous ont permis d'augmenter le débit de la caméra, d'analyser ses
performances et d'évaluer l'impact des retards de mesure causés par l'extraction
de caractéristiques visuelles à l'aide de techniques de vision par ordinateur.

La réalisation de ce simulateur était basée sur le modèle et les paramètres
dynamiques estimés de notre manipulateur. Une autre contribution de cette thèse
est en effet l'identification précise d'un ensemble de paramètres dynamiques réa-
lisables et un modèle de la friction dynamique des articulations du robot grâce au
cadre proposé pour la récupération des paramètres.
# Structure de la Thèse

Ce travail de thèse est structuré en quatre parties principales. La première (Partie I) contient une introduction a la commande en force des manipulateurs robotiques et a l'asservissement visuel. Ensuite, l'état de l'art sur le couplage de la vision et de la détection de force est passé en revue. La deuxième partie (Partie II) met en évidence nos contributions au couplage de la vision et de la détection de force dans l'espace des caractéristiques visuelles, permettant à un manipulateur robotique d'obtenir activement un mouvement conforme, grâce aux contrôleurs et techniques basés sur des modèles développés et publiés/soumis dans les articles du candidat [5, 6]. La troisième partie (partie III) illustre d'autres contributions apportées à l'identification dynamique du modèle et à la récupération des paramètres d'un manipulateur robotique, qui est un ingrédient clé à la fois pour la conception de contrôleurs avec des performances supérieurs, qui prennent en compte la dynamique du manipulateur comme nous l'avons fait dans la partie II, ainsi que des simulations haute précision. Cette partie de la thèse est basée sur les papiers du candidat [7, 8]. La quatrième partie (Partie IV) contient les conclusions de la thèse et les orientations de recherche futures. Nous rapportons enfin des informations techniques complémentaires dans les annexes relatives au matériel complémentaire de l'article [7] du candidat.

# Aperçu de la partie I

Cette partie contient les préliminaires et les techniques utilisées dans ce travail de thèse ainsi qu'une revue de l'état de l'art sur le sujet.

In Chapter 2 nous fournissons tous les éléments de modélisation nécessaires pour traiter de la commande des manipulateurs. En particulier, des indications sur la cinématique et la dynamique d'un manipulateur série sont données, ainsi que des indications sur l'identification des paramètres dynamiques. Enfin, les schémas classiques de commande de la force sont discutés.

In Chapter 3 nous présentons quelques aspects fondamentaux de la commande visuel classique ainsi qu'une introduction aux modèle de deuxième ordre.

In Chapter 4 nous passons en revue les différents schémas de commande intégré vision-force dans la littérature et soulignons leurs forces et faiblesses. Ceci est suivi d'une discussion sur la façon dont ce travail de thèse se situe par rapport à l'état de l'art.
## Aperçu de la partie II

Cette partie contient les contributions de l'auteur sur le couplage de la vision et de la détection des forces au niveau de la commande.

Au chapitre 5, les résultats de l'asservissement visuel de deuxième ordre pour les deux cas de cibles statiques et mobiles sont présentés et la possibilité d'utiliser de tels contrôleurs pour l'interaction avec l'environnement est explorée. La dérivation de contrôleurs d'impédance dans l'espace des caractéristiques pour les deux cas de suivi d'un objet en mouvement ou statique est dérivée ainsi que des filtres de Kalman étendus basés sur la cinématique d'asservissement visuel pour augmenter le taux d'information visuelle et estimer la vitesse de la cible pour les deux cas de PBVS et IBVS avec des points d'image comme caractéristiques visuelles. Des simulations sont effectuées pour valider les performances de l'estimateur lors d'une tâche d'insertion d'une cheville dans un trou sur une pièce mobile. Des expériences sont également menées sur un vrai manipulateur redondant avec une caméra à faible coût montée sur le poignet. Les détails de plusieurs défis rencontrés lors de la mise en œuvre sont également discutés.

Au chapitre 6, nous illustrons le concept de repere conforme dans l'espace des caractéristiques et proposons un framework général, à savoir le schéma de commande de la vision et de la force "Extended External Hybrid", pour combiner les informations visuelles et de force dans l'espace des caractéristiques visuelles de façon générale, puisque le traitement ne dépend pas des caractéristiques visuelles choisies. La vision et la détection de force sont couplées dans l'espace des caractéristiques, évitant à la fois la convergence vers un minimum local et l'apparition d'incohérences au niveau de l'actionnement. Toute direction de l'espace de tâche est commandée simultanément par la vision et la force. La conformité contre les forces d'interaction est obtenue dans l'espace des caractéristiques le long les caractéristiques définissant la tâche visuelle. Des expérimentations sur une plate-forme réelle sont menées pour évaluer l'efficacité du framework proposé.

## Aperçu de la partie III

Cette partie contient les contributions de l'auteur sur l'identification des modèles d'un robot et les outils de simulation développés.

Au chapitre 7, nous abordons le problème de l'extraction d'un ensemble faisable de paramètres dynamiques caractérisant la dynamique d'un robot manipulateur. Nous commençons par identifier par une approche des moindres carrés ordinaires les coefficients dynamiques qui paramétrent linéairement le modèle. À
partir de ceux-ci, nous récupérons un ensemble de paramètres de liaison faisables (masse, position du centre de masse, inertie) qui sont fondamentaux pour des simulations dynamiques plus réalistes ou lors de la mise en œuvre en temps réel de lois de commande de robots à l'aide d'algorithmes récursifs de Newton-Euler. Le problème résultant est résolu au moyen d'une méthode d'optimisation qui intègre des contraintes sur la cohérence physique des paramètres dynamiques, y compris l'inégalité triangulaire des tenseurs d'inertie des liens ainsi que d'autres contraintes définies par l'utilisateur, éventuellement non linéaires. En tant que cas d'utilisation, l'approche est développée pour notre manipulateur, i.e., le robot Panda de Franka Emika, identifiant pour la première fois ses coefficients dynamiques, un modèle précis de friction des articulations et un ensemble de paramètres dynamiques faisables. La validation du modèle dynamique identifié et des paramètres faisables récupérés est présentée pour le problème de dynamique inverse en utilisant, respectivement, une approche lagrangienne et des calculs de Newton-Euler.

Au chapitre 8, nous présentons un nouveau simulateur open source basé sur ROS et CoppeliaSim pour le célèbre robot Franka Emika Panda entièrement intégré dans l'écosystème ViSP, une puissante bibliothèque pour Visual-Servoing. Le simulateur présente le modèle dynamique qui a été identifié avec précision dans le chapitre 7 à partir d'un vrai robot Panda, conduisant à des simulations plus réalistes. L'API C++ est une réplique étendue de la classe dans ViSP du vrai robot, permettant de réduire l'écart entre le code de simulation et le déploiement réel du logiciel de commande. Conçu comme une plate-forme de simulation de recherche polyvalente, il est bien adapté pour le prototypage rapide et le test d'applications d'asservissement visuel ainsi que, en général, pour tout objectif pédagogique sur les manipulateurs robotiques. Tous les logiciels, modèles et scènes CoppeliaSim présentés dans cet ouvrage sont accessibles au public sous licence libre GPL-2.0.

## Aperçu de la partie IV

Cette dernière partie, énonce les conclusions de la thèse et dessine quelques pistes de recherches futures possibles.

Le chapitre 9 synthétise les conclusions de ce travail de thèse et résume les principales contributions apportées à l'état de l'art. En outre, des directions futures possibles qui mériteraient d'être explorées sont discutées et quelques questions ouvertes sont indiqués.
# Aperçu des Annexes

Dans l'annexe A nous comparons les résultats de l'identification des paramètres dynamiques obtenus à la fois avec notre méthode et avec l'approche classique qui utilise des trajectoires d'excitation. Dans l'annexe B nous montrons un exemple de l'utilisation de notre framework d'optimisation pour la récupération d'un ensemble réalisable de paramètres dynamiques en utilisant des contraintes non linéaires et conditionnelles sur un robot simulé 2R.

# Publications connexes

Nos travaux ont abouti aux contributions suivantes :

* C. Gaz, M. Cognetti, **A. Oliva**, P. Robuffo Giordano, and A. De Luca, "Dynamic identification of the Franka Emika Panda robot with retrieval of feasible parameters using penalty-based optimization," *IEEE Robotics and Automation Lett.*, 2019

Supplementary material and accompanying video :

https://ieeexplore.ieee.org/document/8772145/media#media.

* **A. A. Oliva**, P. Robuffo Giordano, and F. Chaumette, "A general visual-impedance framework for effectively combining vision and force sensing in feature space", *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp.4441-4448, 2021.

Video : https://www.youtube.com/watch?v=Qw0Dnhq2uRQ

* **A. A. Oliva**, E. Aertbeliën, J. de Schutter, P. Robuffo Giordano, and F. Chaumette, "Towards dynamic visual servoing for interaction control and moving targets," ICRA 2022 - *IEEE International Conference on Robotics and Automation*, May 2022, Philadelphia, United States. (to appear)

Video : https://www.youtube.com/watch?v=fOSRnxd1dlE

* **A. A. Oliva**, F. Spindler, P. Robuffo Giordano, and F. Chaumette, "Franka-Sim : A Franka Emika's Panda robot simulator with visual-servoing enabled capabilities," 2022 *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. (submitted)

Video : https://www.youtube.com/watch?v=Kxn3pXsK9h4&t=2s
To Tato...

« In fisica e in matematica è impressionante la sproporzione tra lo sforzo per capire una cosa nuova per la prima volta e la semplicità e naturalezza del risultato una volta che i vari passaggi sono stati compiuti. Nel prodotto finito, nelle scienze come nella poesia, non c'è traccia della fatica del processo creativo e dei dubbi e delle esitazioni che lo accompagnano. »

« In physics and mathematics, the disproportion between the effort to understand something new for the first time and the simplicity and naturalness of the result once the various steps have been completed is striking. In the finished product, in the sciences as in poetry, there is no trace of the effort of the creative process and the doubts and hesitations that accompany it. »

Giorgio Parisi, Physics Nobel Prize 2021.
Acknowledgments

This doctoral journey now draws to an end after a long trip that begun even
before I obtained my doctoral position in this fantastic laboratory and set foot
on Breton land. This manuscript summarizes and encapsulates the scientific and
experimental work that I have done over the last three and a half years in a
formal and concise manner, but the narrative style that is adopted in this type of
literature leaves no room for recounting the vicissitudes and experiences that one
faces both inside and outside of the laboratory. So I would like in this anecdote
to thank everyone who, in one way or another, contributed to the achievement of
this thesis work.

Robots have fascinated me since I was a child and I knew that the way forward
was toward understanding the laws that govern them and make them seem en-
dowed with life.... but passions sometimes succumb to more imperative reasons,
especially if pockets have a say; so, after university I began to work in indus-
try and began, reluctantly and in my late twenties, to get used to the idea that
maybe, in my spare time and as a hobby, I could devote myself to some “Maker”
projects in robotics. Then, one day, I was in L’aquila and went to visit my very
dear robotics professor, Costanzo Manes, whom I owe enormous gratitude to for
the extremely inspiring and motivating discussion we had that day, encouraging
me to seek what is truly important to me, and I must say that my personal and
professional life has never been the same since.

The transition to research occurred gradually. I first arrived beyond the Alps
as a research engineer at Inria (Grenoble), an institution to which I am eternally
grateful for everything it has given me over the years and which has always made
me feel like part of a big family. There, under the supervision of my friend Agostino
Martinelli (a.k.a. “Massiccio”), I took my first steps into the fascinating world of
research. The conversations we used to have about mountains and science until I
left for Rennes, also with Alessandro Renzaglia, were pure inspiration.

At the heart of these acknowledgements must be two extraordinary people and
researchers, my supervisors: Paolo Robuffo Giordano and François Chaumette,
whom, in addition to always being present and available (even late at night), gave
me complete freedom in conducting research and created the optimal conditions
for allowing me to complete my doctorate, taking also into account the not trivial
historical context we faced. It was an honour and a privilege to be under their
supervision.

Much of the work I conducted in the lab was experimental, and this would
have been really arduous without the help and support of Fabien Spindler, with
whom it was always pleasant to work with and see in action working flawlessly.

During my third year, I had the opportunity to visit KU Leuven for a brief
but rewarding period owing to collaboration with Prof. Joris de Schutter and
Erwin Aertbeliën, whom I thank for welcome me to their laboratory and for the
stimulating discussions.

At this point, the list of people who must be thanked for any scientific or non-
scientific conversation is extensive. I'd like to thank Marco Cognetti in particular
because he always kept his office door open for any questions I had during my
first year of PhD. Thank you also to Claudio Pacchierotti and Marco Aggravi
for countless tips and assistance, to Claudio Gaz for the fruitful collaboration, to
Alberto Jovane, master of blender and Adam Khayam, wizard of any programming
paradigm. Thanks to Nicola De Carli and John Thomas, great office neighbours
for insightful talks... just thank you to all the RAINBOWERS, I cannot name
you all.

I would also like to thank my family for their unconditional support, regardless
of any decisions I may make in my life.

Last but not least, and as is always the case in these acknowledgements, one
tends to leave last to the person who had a substantial weight in the success of
the thesis, because without that person one should have had cloned oneself to be
able to lead a "normal" life. And you thank that person because has always been
by your side, every day to talk to you, because has always listened to you carefully
and knows every point of your research even if you never showed an equation. You
also thank that person for being able to give you your space when you were under
dead-line during holidays, a special event or even a PACS. For all these reasons
and thousands more, I thank you Naty.

I hereby acknowledge the support of the Brittany region during my research
visit at the KU Leuven.
3 Vision Control 37

<table><tr><td>3.1 Introduction</td><td>38</td></tr><tr><td>3.2 Camera model</td><td>39</td></tr><tr><td>3.3 Visual Servoing</td><td>40</td></tr><tr><td>3.3.1 IBVS: Image-Based Visual Servoing</td><td>42</td></tr><tr><td>3.3.2 PBVS: Pose-Based Visual Servoing</td><td>43</td></tr><tr><td>3.3.3 Stability analysis</td><td>45</td></tr><tr><td>3.3.3.1 Stability analysis in the IBVS case</td><td>45</td></tr><tr><td>3.3.3.2 Stability analysis in the PBVS case</td><td>46</td></tr><tr><td>3.3.4 Mounted camera</td><td>46</td></tr><tr><td>3.3.5 Feature trajectory tracking</td><td>47</td></tr><tr><td>3.3.6 Target tracking</td><td>47</td></tr><tr><td>3.4 Second-order Visual Servoing</td><td>47</td></tr><tr><td>3.5 Conclusion</td><td>49</td></tr></table>

4 Vision-Force control 51

<table><tr><td>4.1 Introduction</td><td>52</td></tr><tr><td>4.2 Hybrid Vision-Force control</td><td>53</td></tr><tr><td>4.3 Visual-Impedance control</td><td>54</td></tr><tr><td>4.4 External Hybrid Vision-Force control</td><td>55</td></tr><tr><td>4.5 Constraint-based methods</td><td>58</td></tr><tr><td>4.6 Technological aspects</td><td>60</td></tr></table>

Part II Feature Space Compliance 63

5 Feature Space Impedance 65

<table><tr><td>5.1 Introduction</td><td>66</td></tr><tr><td>5.2 Manipulator model in feature space</td><td>67</td></tr><tr><td>5.3 Impedance control in feature space for a static target</td><td>68</td></tr><tr><td>5.4 Impedance control in feature space for a moving target</td><td>72</td></tr><tr><td>5.5 Increasing the visual data rate and estimating the target's motion</td><td>73</td></tr><tr><td>5.5.1 EKF for IBVS with image points features</td><td>74</td></tr><tr><td>5.5.2 EKF for PBVS</td><td>75</td></tr><tr><td>5.6 Simulations</td><td>76</td></tr><tr><td>5.6.1 Task description and controllers implementation</td><td>77</td></tr><tr><td>5.6.2 Simulation Results</td><td>78</td></tr><tr><td>5.7 Real experiments</td><td>82</td></tr><tr><td>5.7.1 Implementation issues</td><td>82</td></tr></table>
<table><tbody><tr><td>5.7.1.1</td><td>Data rate</td><td>83</td></tr><tr><td>5.7.1.2</td><td>Ill-condition of the task Jacobian</td><td>84</td></tr><tr><td>5.7.1.3</td><td>Joints friction</td><td>85</td></tr><tr><td>5.7.1.4</td><td>Illumination conditions</td><td>85</td></tr><tr><td>5.7.2</td><td>Results</td><td>85</td></tr><tr><td>5.8</td><td>Summary</td><td>87</td></tr><tr><td><b>6</b></td><td><b>Feature Space Admittance</b></td><td><b>89</b></td></tr><tr><td>6.1</td><td>Introduction</td><td>90</td></tr><tr><td>6.2</td><td>Related works</td><td>90</td></tr><tr><td>6.3</td><td>Feature Space Admittance</td><td>92</td></tr><tr><td>6.4</td><td>The Extended External Hybrid Vision-Force Control Scheme</td><td>94</td></tr><tr><td>6.4.1</td><td>Force regulation</td><td>95</td></tr><tr><td>6.5</td><td>Stability analysis</td><td>96</td></tr><tr><td>6.6</td><td>Experiments</td><td>97</td></tr><tr><td>6.6.1</td><td>Experimental Setup</td><td>97</td></tr><tr><td>6.6.2</td><td>Peg-in-Hole Experiment</td><td>99</td></tr><tr><td>6.6.3</td><td>Extended External Hybrid vs External Hybrid</td><td>101</td></tr><tr><td>6.7</td><td>Fictitious forces</td><td>103</td></tr><tr><td>6.8</td><td>Summary</td><td>104</td></tr></tbody></table>

## Part III Manipulator dynamic model and simulation 105

<table><tbody><tr><td><b>7</b></td><td><b>Dynamic model of the Franka Emika's Panda robot</b></td><td><b>107</b></td></tr><tr><td>7.1</td><td>Introduction</td><td>108</td></tr><tr><td>7.2</td><td>The Panda robot</td><td>110</td></tr><tr><td>7.3</td><td>Identification procedure</td><td>111</td></tr><tr><td>7.3.1</td><td>Friction estimation</td><td>113</td></tr><tr><td>7.4</td><td>Retrieval of feasible parameters</td><td>114</td></tr><tr><td>7.5</td><td>Results</td><td>117</td></tr><tr><td>7.5.1</td><td>Validation on a physics simulator</td><td>121</td></tr><tr><td>7.6</td><td>Summary</td><td>125</td></tr><tr><td><b>8</b></td><td><b>FrankaSim</b></td><td><b>129</b></td></tr><tr><td>8.1</td><td>Introduction</td><td>130</td></tr><tr><td>8.2</td><td>Related Works</td><td>130</td></tr><tr><td>8.3</td><td>The simulator: FrankaSim</td><td>132</td></tr><tr><td>8.3.1</td><td>Kinematics</td><td>132</td></tr></tbody></table>
<table>
  <tbody>
    <tr>
      <td>8.3.2</td>
      <td>Dynamics</td>
      <td>133</td>
    </tr>
    <tr>
      <td>8.3.3</td>
      <td>ViSP</td>
      <td>135</td>
    </tr>
    <tr>
      <td>8.3.4</td>
      <td>Visp_<br>ros</td>
      <td>136</td>
    </tr>
    <tr>
      <td>8.3.5</td>
      <td>CoppeliaSim</td>
      <td>136</td>
    </tr>
    <tr>
      <td>8.3.6</td>
      <td>Software Architecture</td>
      <td>137</td>
    </tr>
    <tr>
      <td>8.4</td>
      <td>Experiments</td>
      <td>138</td>
    </tr>
    <tr>
      <td>8.4.1</td>
      <td>Single-Arm Experiment: Real vs Simulated</td>
      <td>138</td>
    </tr>
    <tr>
      <td>8.4.2</td>
      <td>The Dual-Arm Experiment</td>
      <td>140</td>
    </tr>
    <tr>
      <td>8.5</td>
      <td>Summary</td>
      <td>141</td>
    </tr>
    <tr>
      <td colspan="2"><strong>Part IV Conclusions and future directions</strong></td>
      <td><strong>143</strong></td>
    </tr>
    <tr>
      <td><strong>9</strong></td>
      <td><strong>Conclusion</strong></td>
      <td><strong>145</strong></td>
    </tr>
    <tr>
      <td>9.1</td>
      <td>Summary and contributions</td>
      <td>145</td>
    </tr>
    <tr>
      <td>9.2</td>
      <td>Open issues and future perspectives</td>
      <td>147</td>
    </tr>
    <tr>
      <td colspan="2"><strong>Appendix A Identification procedure comparison</strong></td>
      <td><strong>149</strong></td>
    </tr>
    <tr>
      <td>A.1</td>
      <td>Comparison of dynamic coefficients</td>
      <td>149</td>
    </tr>
    <tr>
      <td>A.2</td>
      <td>Comparison of dynamic parameters</td>
      <td>152</td>
    </tr>
    <tr>
      <td colspan="2"><strong>Appendix B Parameters retrieval using nonlinear and conditional<br>constraints</strong></td>
      <td><strong>157</strong></td>
    </tr>
    <tr>
      <td colspan="2"><strong>Bibliography</strong></td>
      <td><strong>163</strong></td>
    </tr>
  </tbody>
</table>
CHAPTER

1

Introduction

Contents

<table><tr><td>1.1</td><td>On Vision-Force coupling: Motivation</td><td>2</td></tr><tr><td>1.2</td><td>Challenges</td><td>5</td></tr><tr><td>1.3</td><td>Contributions</td><td>6</td></tr><tr><td>1.4</td><td>Thesis structure</td><td>7</td></tr><tr><td>1.5</td><td>Related publications</td><td>11</td></tr></table>
1.1 On Vision-Force coupling: Motivation

Robotics, the term that Isaac Asimov assumed in his short story “Liar!” in 1941, refers to the science and technology studying robots, the mechanical machines he imagined devoid of feelings and with a “positronic” brain programmed by man. The term robot, on the other hand, appeared for the first time twenty years earlier (1920) in the science-fiction play “R.U.R. (Rossum’s Universal Robots)” by the Czech writer Karel Čapek in which artificial biological organisms, that may be mistaken for humans, were assigned the task of replacing human beings in subordinate jobs. This word, coined by Karel Čapek’s brother Josef, comes from the Czech “robota” which means indeed “hard work” or “forced labour” and is derived from rab, meaning “slave”. Even though this “modern” term dates back to a century ago, our desire to build machines capable of replacing humans in any disparate situation or in tedious, repetitive, or hazardous tasks has much more ancient roots, today more fervent than ever.

<figure><img src="image_5.png" /><figcaption>**Figure 1.1** – From Greek mythology to date: (a) Obverse of silver didrachma from Phaistos depicting Talos (“TΑΛΩΝ”),~ 400 B.C.. (b) The Leonardo Da Vinci’s “Automaton knight” ~1495. (c) Unimate: First industrial robot, 1961. (d) Boston Dynamics’ Atlas: the next generation, 2016.</figcaption></figure>
In the Greek mythology from the time of Homer, Talos was a bronze giant, forged by Hephaestus, the blacksmith god of technology, to protect Europa in Crete. The word automaton, “self-moving”, was first used by Homer (750-650 B.C.) to describe the many animated devices fabricated by Hephaestus. Figure 1.1a shows the obverse of a silver didrachma depicting Talos (“TAAΩN”). Figure 1.1b shows the Leonardo da Vinci’s “Automaton knight”, a humanoid automaton. In 1961, George Devol invented the world’s first industrial robot “Unimate”, shown in Figure 1.1c. Finally, Figure 1.1d shows Atlas: the next generation, developed by Boston Dynamics in 2016; it is likely to be the most advanced robot ever built to date, being able to parkour on an obstacle track as well as perform back-flips.

Modern factories are making large use of robotic manipulators to perform very specific, restricted and deterministic tasks, e.g., *welding*, *pick-and-place or spraying* (as shown in Figure 1.2), for which the exact sequence of the operations to be performed is perfectly known *a priori*. With these restrictions and considering the highly structured and well-controlled environments in which these robots are routinely used, the design of the task control becomes relatively simple resorting to path/trajectory planning techniques, which will benefit from the excellent kinematic capabilities of industrial robots and the available prior knowledge of the environment.

<figure><img src="image_6.png" /><figcaption>**Figure 1.2** – (a) Tesla giga-factory assembly line: at each station several manipulators equipped with arc welding machines apply the preprogrammed welding points. (b) Path planned for an automated car-body spraying operation. (c) A SCARA robot performing pick-and-place operations in a well-structured environment.</figcaption></figure>
These robots have been used as mere multipurpose automatons, to automate
industrial production processes under the justification of greater productivity and
quality control, without really exploiting their tremendous flexibility compared to
fixed automated machines; this is also due to the fact that industrial manipula-
tors are unsophisticated and simple-minded with very poor decision making skills
right out of the box (commercial industrial robots are typically provided with
*proprioceptive* joint position sensors only). Indeed, flexibility is the real strength
that distinguishes *man*, the most versatile manipulator of nature, from machines.
This great flexibility derives in part from the ability to process multiple sensory
information; hence, our interest in projecting these sensing abilities into indus-
trial robots, being essential for reaching higher levels of flexibility and autonomy.
This will enable even small-sized companies with limited production volumes and
different products to benefit from this technology. The possibility of this scenario
was already envisioned by Bajcsy [1] four decades ago. The rise of the so-called
*Cobots*, or Collaborative Robots, are indeed one of the key technologies that are
driving the modern industrial revolution, also known as industry 4.0. *Cobots* are
a driving technology that is pushing hard on the *smart production* front, That is,
the forefront of new manufacturing technologies that foster collaboration among
all the elements involved in the manufacturing process, i.e., collaboration among
operators, machines, and tools.

Although path/trajectory planning approaches have proved their effectiveness
in well-structured applications, which among other things require a considerable
modeling and calibration engineering effort, those methods are hardly applicable
to the highly dynamic scenarios illustrated above, which could even involve vol-
untary or accidental contacts with a human operator or the environment. *Sensor-
based* control, on the other hand, offers more flexibility in dealing with the uncer-
tainties that characterize unstructured/dynamic or even non-rigid environments.

Of all the available sensing modalities, vision and force sensing are among
the most complementary and relatively inexpensive ones. Cameras are capable
of providing a rich description of the scene in which the manipulator moves, al-
lowing, for example, to accurately align the manipulator's end-effector with an
object within an imprecisely calibrated or dynamically changing environment. In
contrast, force sensors provide much more accurate, albeit localised, contact in-
formation, enabling the control task to be performed with greater precision than
vision alone. With no surprise, vision and force sensing are the most widely
adopted sensory configuration in the context of physical human-robot interaction
(pHRI) among the sensing modalities (i.e., vision, force(tactile), audition and
distance) recently surveyed by Cherubini et al. [9].
Before delving into the difficulties that multi-sensory integration entails, the scientific community has sought to address the need to describe and characterize the control qualities of each specific sensory modality. Early vision and force sensing research focused on identifying the strengths and shortcomings of the control techniques associated with each of these sensors independently. Indeed, two strong communities, that of force control and the visual-servoing one, have achieved a number of significant breakthroughs in the past decades, bringing their respective fields to a state of relatively advanced maturity. This thesis is halfway between these two fields and aims at providing solutions to the challenging problem of the coupling of vision and force sensing by proposing both innovative combined control strategies as well as the tools needed to support the study, e.g., in simulation, and the implementation of those strategies.

Two decades ago, force and/or vision sensors were still an exception in industrial settings and were mostly confined to the field of scientific research [2].

The need to develop novel integrated vision-force control techniques that are safer, robust and highly performing is also driven by the ever increasing adoption of *Cobots* in small and medium-sized businesses, where robots find themselves sharing the workspace with human operators and other machinery.

## 1.2 Challenges

There is of course significant interest in integrated vision-force approaches, as the development of combined vision-force applications is just a natural step in the evolution of sensor-based robotics [2], allowing the advantages of both sensors to be combined and the individual weaknesses to be overcome. However, extra needs arise as a result of the integration process, and attaining an effective combined use of vision and force is not straightforward. Vision and force sensors measure fundamentally different quantities, which leads to inherent difficulties when trying to combine data from the two sensors. Sensor integration approaches need a common representation of the sensory input to be integrated, but force and vision sensors do not share such a common representation of the data they provide. Furthermore, due to their distinct nature, the two sensors are often only useful at distinct stages of the task at hand. On the other hand, the most common force control schemes make use of wrist-mounted force/torque sensors, which are capable of measuring any kind of force: gravitational, inertial or contact force. The inertial coupling of the end-effector mass (and eventual payload) positioned downstream of the sensor introduces a fundamental problem when the controlled directions are shared between vision and force sensing. Further aspects character-
izing both sensors are the rates at which they can provide samples. Force/torque
sensors usually provide very high rate measurements (1 kHz) that require little
processing or interpretation for being used within a feedback control loop. Vi-
sion sensors, on the other side, run at much slower rates (typically 30 to 60 fps)
and their measurements require high levels of processing for being usable in a
control algorithm, which introduces non negligible latency in the availability of
the control signal. Considering that the servo rate of a manipulator must be at
least ten times greater than its mechanical resonance frequency [3], the restricted
feedback rate in vision systems is found to be the main issue restricting the appli-
cability of visual-servoing to real-world robot operations and its integration with
conventional sensor feedback systems that generate commands directly for the
low-level controller [4]. This is the case when one wants to take the manipulator's
dynamics into account in the controller design. Finally, simplifying the models
used to describe the system, such as neglecting certain phenomena that are diffi-
cult to model, e.g. friction, can in practice have significant effects on the system
behaviour with consequent tracking performance degradation.

In this thesis we address several of these challenges such as:

* sensory integration, seen from the control perspective,

• increasing the sampling rate of visual information,

* mitigating the negative effects related to data latency, and

* system modelling and parameter identification.

1.3 Contributions

In this thesis we have contributed with the synthesis of controllers that realise
the sensory integration of vision and force directly in the sensor space, i.e., in
the visual feature space. These controllers achieve the highest degree of sensory
integration, as they allow to control every possible direction with both sensory
modalities.

In particular, one of the first contributions of this work was the mapping of
the physical forces acting on the manipulator into the visual feature space. This
allowed to derive, on one hand, an impedance control in the visual feature space,
and on the other hand to extend the concept of compliant frame into the feature
space. The former is an inverse dynamics controller, which allows the dynamics
of the manipulator to be taken into account in the controller synthesis which do
not require, in its simplest version, any force/torque sensor in order to operate.
Thanks to the mapping of forces into the visual feature space, it was possible to
define an admittance law in such space, which we used to extend the capabilities
of one of the vision/force control schemes in the literature.

Another aspect we tackled was the narrowing of the rate discrepancy between
sensors. The use of a Bayesian estimation framework allowed us to virtually
increase the availability of visual data at a higher rate and also to mitigate the
negative effects due to the latency of computer vision algorithms. The proposed
estimation models also allowed to reconstruct the velocity of a moving target
observed by the camera. These estimators can be easily used in different control
schemes.

The different control schemes developed in this work were validated through
extensive simulations and experiments on a real platform in our laboratory. For
what concerns the simulations, in this work we have also developed a physics co-
simulation environment that allows not only to rapidly prototype visual servoing
applications, but also to perform dynamic simulations with physical interaction
with the environment. Thanks to this simulator, it was possible to study step by
step all the problems and limitations of second-order visual servoing (a.k.a. Dy-
namic Visual Servoing) controllers. We could also design and test the estimators
that allowed us to increase the camera's data rate, analyse its performance, and
assess the impact of delayed measurements caused by visual feature extraction
using computer vision techniques.

The realisation of this simulator was based on the model and estimated dy-
namic parameters of our manipulator. Another contribution of this thesis is indeed
the accurate identification of a set of feasible dynamic parameters and a model
of the dynamic friction of the robot joints thanks to the proposed framework for
parameters retrieval.

1.4 Thesis structure

This thesis work is organized in four main parts. The first one (Part I) contains
introductory background to the force control of robotic manipulators and visual
control. Then, the state-of-the-art on the coupling of vision and force sensing is
reviewed. The second part (Part II) highlights our contributions to the coupling
of vision and force sensing in the visual feature space, allowing a robotic manipu-
lator to actively achieve compliant motion, thanks to the model-based controllers
and techniques developed and issued in the following author's publications [5, 6].
The third part (Part III) illustrates further contributions made on the dynamic
model identification and parameter retrieval for robotic manipulators, which is
a key ingredient both for designing superior controllers which take into account
the dynamics of the manipulator, as we did in Part II, and for performing more
accurate simulations. This part of the thesis is based on the author's contribu-
tions [7, 8]. In the fourth part (Part IV) of the manuscript, we state the thesis
conclusions and give some future research directions. We finally report comple-
mentary technical information in the appendices relative to the supplementary
material of the author's paper [7].

**Outline of Part I**

This part contains the preliminaries and techniques involved in this thesis and a
state-of-the-art review on the subject.

In Chapter 2 we provide all the necessary modelling background to deal with
the control of manipulators. In particular, some elements of the kinematics and
dynamics of a serial manipulator are given, as well as elements for the identification
of dynamic parameters. Finally, classical force control schemes are discussed.

In Chapter 3 we give an introduction to visual servoing control techniques for
both the cases of 2D and 3D features, showing some generalities and deriving the
kinematic control law. We then present some more advanced schemes for both
trajectory and target tracking and discuss the stability of the classic cases. We
conclude with the derivation of the second-order visual servoing.

In Chapter 4 we review the different integrated vision-force control schemes present in the literature, highlighting their own strengths and weaknesses. This is followed by a discussion on how this thesis work places itself with respect to the state-of-the-art.

**Outline of Part II**

This part contains the author's contributions on the coupling of vision and force
sensing at control level.

In Chapter 5, we present our results on second order visual servoing for both
the cases of static and moving targets while we explore the possibility of using
such controllers for physical interaction with the environment. We firstly derive
the feature space impedance controllers for tracking both motionless or moving
objects and then, we introduce two Extended Kalman Filters (EKF), based on the
visual servoing geometric model, for increasing the rate of the visual information
and estimating the target's velocity. This filters are derived for both Pose-Based
Visual Servoing (PBVS) and Image-Based Visual Servoing (IBVS) with image
points as visual features. Simulations are carried out to validate the estimator
performance during a dynamic Peg-in-Hole insertion task with a moving part.
Experiments are also conducted on a real redundant manipulator with a low-
cost wrist-mounted camera. Details on several issues encountered during the
implementation are also discussed.

In Chapter 6 we illustrate the concept of compliant frame in the visual feature
space and we propose a general framework, namely the *Extended External Hybrid*
vision-force control scheme, for combining visual and force information in the vi-
sual feature space in a general fashion, since the treatment does not depend on the
chosen visual features. Vision and force sensing are coupled in the feature space,
avoiding both the convergence to a local minimum and the arising of inconsisten-
cies at the actuation level. Any task space direction is simultaneously controlled
by both vision and force. Compliance against interaction forces is achieved in
feature space, along the features defining the visual task. Experiments on a real
platform are carried out to evaluate the effectiveness of the proposed framework.

## Outline of Part III

This part contains the author's contributions on the identification of robot models
and the developed simulation tools.

In Chapter 7 we address the problem of extracting a feasible set of dynamic
parameters characterizing the dynamics of a robot manipulator. We start by
identifying through an ordinary least squares approach the dynamic coefficients
that linearly parametrize the model. From these, we retrieve a set of feasible link
parameters (mass, position of center of mass, inertia) that is fundamental for more
realistic dynamic simulations or when implementing in real time robot control
laws, using recursive Newton-Euler algorithms. The resulting problem is solved
by means of an optimization method that incorporates constraints on the physical
consistency of the dynamic parameters, including the triangle inequality of the link
inertia tensors as well as other user-defined, possibly nonlinear, constraints. As use
case, the approach is developed for our manipulator, i.e., the popular Panda robot
by Franka Emika, identifying for the first time its dynamic coefficients, an accurate
joint friction model, and a set of feasible dynamic parameters. Validation of the
identified dynamic model and of the retrieved feasible parameters is presented
for the inverse dynamics problem using, respectively, a Lagrangian approach and
Newton-Euler computations.

In Chapter 8 we present a new open-source simulator based on ROS and
CoppeliaSim for the Franka Emika Panda robot fully integrated in the ViSP [10] ecosystem, a powerful library for Visual-Servoing. The simulator features the dynamic model that has been accurately identified in the previous Chapter 7 from a real Panda robot, leading to more realistic simulations. The C++ API is an extended replica of the ViSP class of the real robot allowing to narrow the gap between simulation code and real control software deployment. Conceived as a multipurpose research simulation platform, it is well suited for fast prototyping and test of visual servoing applications as well as, in general, for any pedagogical purpose in robotic manipulators. All the software, models and CoppeliaSim scenes presented in this work are publicly available under free GPL-2.0 license.

## Outline of Part IV

This part states the thesis conclusions and draws some possible future research lines.

Chapter 9 synthesizes the conclusions of this thesis work and summarises the main contributions made to the state-of-the-art. In addition, possible future research directions that would be worth exploring are discussed and some open questions are listed.

## Outline of the Appendices

In Appendix A we compare the results of the dynamic parameter identification obtained with our method and with the classical approach that uses exciting trajectories. In Appendix B we show an example about the use of our optimization framework for the retrieval of a feasible set of dynamic parameters using nonlinear and conditional constraints on a 2R simulated robot.
## 1.5 Related publications

The following contributions were obtained during the course of this thesis work:

* C. Gaz, M. Cognetti, **A. Oliva**, P. Robuffo Giordano, and A. De Luca, "Dynamic identification of the Franka Emika Panda robot with retrieval of feasible parameters using penalty-based optimization," *IEEE Robotics and Automation Lett.*, 2019

Supplementary material and accompanying video:

https://ieeexplore.ieee.org/document/8772145/media#media

* **A. A. Oliva**, P. Robuffo Giordano, and F. Chaumette, "A general visual-impedance framework for effectively combining vision and force sensing in feature space", *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp.4441-4448, 2021.

Video: https://www.youtube.com/watch?v=Qw0Dnhq2uRQ

* **A. A. Oliva**, E. Aertbeliën, J. de Schutter, P. Robuffo Giordano, and F. Chaumette, "Towards dynamic visual servoing for interaction control and moving targets," ICRA 2022 - *IEEE International Conference on Robotics and Automation*, May 2022, Philadelphia, United States. (to appear)

Video: https://www.youtube.com/watch?v=fOSRnxd1d1E

* **A. A. Oliva**, F. Spindler, P. Robuffo Giordano, and F. Chaumette, "FrankaSim: A Franka Emika's Panda robot simulator with visual-servoing enabled capabilities," 2022 *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. (submitted)

Video: https://www.youtube.com/watch?v=Kxn3pXsK9h4&t=2s
# Part I Preliminaries and State of the Art
CHAPTER

2

Force control of
manipulators

Contents

<table><tr><td>2.1</td><td>Introduction</td><td>16</td></tr><tr><td>2.2</td><td>Manipulator kinematics</td><td>16</td></tr><tr><td>2.2.1</td><td>Differential kinematics</td><td>19</td></tr><tr><td>2.2.2</td><td>Statics</td><td>21</td></tr><tr><td>2.2.3</td><td>Twist and Wrench transformations</td><td>22</td></tr><tr><td>2.3</td><td>Dynamic model</td><td>23</td></tr><tr><td>2.3.1</td><td>Parameter identification</td><td>24</td></tr><tr><td>2.3.2</td><td>Physical consistency of the dynamic parameters</td><td>26</td></tr><tr><td>2.3.3</td><td>Dynamic model in operational space</td><td>27</td></tr><tr><td>2.4</td><td>Force Control Schemes</td><td>28</td></tr><tr><td>2.4.1</td><td>Hybrid position/force control</td><td>28</td></tr><tr><td>2.4.2</td><td>Impedance control</td><td>30</td></tr><tr><td>2.4.3</td><td>Admittance control</td><td>33</td></tr><tr><td>2.5</td><td>Conclusion</td><td>35</td></tr></table>
## 2.1 Introduction

For many real-world robotic applications, interaction with the environment is a fundamental requirement and the ability of robots in managing this interaction often determines the successful execution of a task. For example, assembly or polishing require to control the exchanged forces at contact by regulating them to a specific value. The contact wrench between the end-effector and the environment is the most complete and effective quantity describing the state of the interaction, which is naturally described in the operational space [11].

For implementing an interaction control, either the exact knowledge of the location and geometry of the environment is required for accurately planning the task trajectory, or the robot needs to be equipped with force sensing capabilities to adapt to uncertainties and avoid high contact forces along the constrained directions. An effective way to deal with constrained motions is via active compliance, which can be achieved through impedance control [12] by imposing a *mass-spring-damper* behavior of the robot in contact with the environment. This scheme, as well as *compliance* or *stiffness* control, belongs to the category of *indirect* force control methods [13], since they achieve *open-loop* force control via *closed-loop* position control. Many other approaches have been explored in the past decades for including force sensing capabilities inside a motion control scheme, such as the widely studied *Hybrid Position/Force* [14] that is based on the task description [15], the parallel scheme [16] or, an approach based on external force loops [17]. The ability of these methods to perform *closed-loop* force control via explicit closure of a force feedback loop places them among the *direct* force control methods [13].

## 2.2 Manipulator kinematics

A manipulator can be mechanically described as a kinematic chain consisting of rigid bodies (links) connected by means of revolute or prismatic joints, which constitute its degrees of freedom (DoF). One end of the chain is bounded to a base (or the floor) while at the other extreme a tool is usually attached (see Figure 2.1). The overall motion of the structure is achieved through the composition of elementary motions of each link with respect to the previous one due to the motion of the $q_i$-th joint ($\forall i = 1, ..., n$). In order to manipulate the tool end point (end-effector), the description of the *pose* ($\mathbf{x}_e = [^f\mathbf{p}_e ^f\mathbf{\phi}_e]^\top$), *position* ($^f\mathbf{p}_e$) and *orientation* ($^f\mathbf{\phi}_e$), of such point is required.
This is referred to as the direct geometric model [13]

$$ \boldsymbol{x}_e = \mathcal{K}(\boldsymbol{q}) \qquad (2.1) $$

being $\boldsymbol{q} \in \mathbb{R}^n$ the vector of joint variables. To the pose vector ($\boldsymbol{x}_e$) is associated an homogeneous transformation matrix $^f\boldsymbol{T}_e(^f\boldsymbol{p}_e, ^f\boldsymbol{\phi}_e) \in SE(3) = \mathbb{R}^3 \times SO(3)$ representing the pose of the end-effector frame ($\boldsymbol{\Sigma}_e$) expressed in floor frame ($\boldsymbol{\Sigma}_f$) (frame placed at the base of the manipulator):

$$ ^f\boldsymbol{T}_e(^f\boldsymbol{p}_e, ^f\boldsymbol{\phi}_e) = \begin{bmatrix} ^f\boldsymbol{R}_e(^f\boldsymbol{\phi}_e) \\ 0 \ 0 \ 0 \end{bmatrix} \begin{bmatrix} ^f\boldsymbol{p}_e \\ 1 \end{bmatrix} = \begin{bmatrix} ^f\boldsymbol{n}_e \ ^f\boldsymbol{s}_e \ ^f\boldsymbol{a}_e \\ 0 \ 0 \ 0 \end{bmatrix} \begin{bmatrix} ^f\boldsymbol{p}_e \\ 1 \end{bmatrix} \quad (2.2) $$

where $^f\boldsymbol{R}_e(^f\boldsymbol{\phi}_e) \in SO(3)$ is the rotation matrix representing the orientation of frame $\boldsymbol{\Sigma}_e$ expressed in frame $\boldsymbol{\Sigma}_f$ coordinates. $^f\boldsymbol{\phi}_e$ is the vector form of the ori-

<figure><img src="image_7.png" /><figcaption>Modified Denavit-Hartenberg parameters</figcaption></figure>

<table><caption>**Figure 2.1** – Panda robot with its kinematic parameters according to the modified Denavit-Hartenberg convention: $d_1 = 0.333$ m, $d_3 = 0.316$ m, $d_5 = 0.384$ m, $d_{fl} = 0.107$ m, $d_e = 0.1034$ m, $a_4 = 0.0825$ m, $a_5 = -a_4$ m, $a_7 = 0.088$ m.</caption><thead><tr><th>i</th><th>a<sub>i</sub></th><th>&alpha;<sub>i</sub></th><th>d<sub>i</sub></th><th>&theta;<sub>i</sub></th></tr></thead><tbody><tr><td>1</td><td>0</td><td>0</td><td>d<sub>1</sub></td><td>q<sub>1</sub></td></tr><tr><td>2</td><td>0</td><td>-&pi;/2</td><td>0</td><td>q<sub>2</sub></td></tr><tr><td>3</td><td>0</td><td>&pi;/2</td><td>d<sub>3</sub></td><td>q<sub>3</sub></td></tr><tr><td>4</td><td>a<sub>4</sub></td><td>&pi;/2</td><td>0</td><td>q<sub>4</sub></td></tr><tr><td>5</td><td>a<sub>5</sub></td><td>-&pi;/2</td><td>d<sub>5</sub></td><td>q<sub>5</sub></td></tr><tr><td>6</td><td>0</td><td>&pi;/2</td><td>0</td><td>q<sub>6</sub></td></tr><tr><td>7</td><td>a<sub>7</sub></td><td>&pi;/2</td><td>0</td><td>q<sub>7</sub></td></tr><tr><td>fl</td><td>0</td><td>0</td><td>d<sub>fl</sub></td><td>0</td></tr><tr><td>e</td><td>0</td><td>0</td><td>d<sub>e</sub></td><td>-&pi;/4</td></tr></tbody></table>

entation representation which could be either one of the 12 possible *Euler angles* representations, or the *Axis/angle*, or again, the *unit quaternion* representation.
Each column of the $^f\mathbf{R}_e(^f\phi_e)$ is a unit vector of the end-effector frame $\Sigma_e$, which is placed conveniently from case to case. When the tool is a two finger gripper, the origin of the frame is placed at the mean point between the fingers and the unit vectors are chosen such that $^f\mathbf{s}_e$ belongs to the plane in which the fingers slide, $^f\mathbf{a}_e$ points in the approach direction towards the object to be grasped, while $^f\mathbf{n}_e$ is normal to the sliding plane (See Figure 2.1).

It is evident that the pose of the end-effector frame depends on the manipulator's configuration ($\mathbf{q} \in \mathbb{R}^n$) and it can be systematically computed as a recursive product of homogeneous transformation matrices ($i^{-1}\mathbf{T}_i(q_i)$) from the end-effector to the base, each of which is a function of a single joint variable ($q_i$). Therefore, the overall coordinate transformation that expresses the pose of the end-effector in base frame for the $n = 7$ joint manipulator in Figure 2.1 is given by

$$ ^f\mathbf{T}_e(\mathbf{q}) = ^f\mathbf{T}_1(q_1) ^1\mathbf{T}_2(q_2) \dots ^6\mathbf{T}_7(q_7) ^7\mathbf{T}_{fl} ^{fl}\mathbf{T}_e \quad (2.3) $$

in which the two rightmost transformations, i.e., from the last joint to the flange ($^7\mathbf{T}_{fl}$) and from flange to end-effector frame ($^{fl}\mathbf{T}_e$), are constant. The reference frames along the kinematic chain are often placed following the *Denavit-Hartenberg* [18] convention (DH). For the Panda robot from Franka Emika in our example, the frames placement follows the *Modified Denavit-Hartenberg* [19] convention, whose DH parameters are also reported in Figure 2.1.

The space in which the pose vector ($\mathbf{x}_e$) is defined is usually the space in which the operations (or tasks), that are requested to the manipulator, are specified; therefore it is called the *operational space* [11], while the space in which the vector of joint variables $\mathbf{q} = [q_1 \dots q_n]^\top$ is defined is referred to as the *joint space* or *configuration space*.

The direct geometric model equation, in both its forms (2.1) or (2.2), expresses the (unique) relationship between the joint variables and the end-effector's pose. The inverse problem, on the other hand, refers to the problem of retrieving the vector of joint variables once an end-effector pose is given. The resolution of this problem is critical in order to transform the motion specifications defined in the operational space for the end-effector, into the corresponding motion in the joint space, allowing the required motion to be realized on the mechanical structure. Furthermore, the inverse problem has no unique solution for redundant manipulators (or, more broadly, when the number of joints is greater that the DoF of the task space).
### 2.2.1 Differential kinematics

We have seen the relationship between the end-effector's posture and the joint variables. In this subsection, we will focus on their differential relationship, or the relationship that exists between the end-effector's linear ($f\dot{\boldsymbol{p}}_e \in \mathbb{R}^3$) and angular ($f\boldsymbol{\omega}_e \in \mathbb{R}^3$) velocity with the joint velocities $\dot{\boldsymbol{q}} \in \mathbb{R}^n$. Both relationships are linear with respect to the joint velocities as follows:

$$ 
\begin{aligned}
f\dot{\boldsymbol{p}}_e &= \boldsymbol{J}_{eP}(\boldsymbol{q})\dot{\boldsymbol{q}} \\
f\boldsymbol{\omega}_e &= \boldsymbol{J}_{eO}(\boldsymbol{q})\dot{\boldsymbol{q}}
\end{aligned} 
\qquad (2.4) 
$$

being $\boldsymbol{J}_{eP} \in \mathbb{R}^{3 \times n}$ and $\boldsymbol{J}_{eO} \in \mathbb{R}^{3 \times n}$ respectively the matrices relative to the contribution of the joint velocities to the linear ($f\dot{\boldsymbol{p}}_e$) and angular ($f\boldsymbol{\omega}_e$) velocities of the end-effector. One can then write the differential kinematic equation in compact form as

$$ 
\begin{aligned}
f \boldsymbol{v}_e = \begin{bmatrix} f \dot{\boldsymbol{p}}_e \\ f \boldsymbol{\omega}_e \end{bmatrix} = \begin{bmatrix} f \boldsymbol{v}_e \\ f \boldsymbol{\omega}_e \end{bmatrix} = \begin{bmatrix} \boldsymbol{J}_{eP}(\boldsymbol{q}) \\ \boldsymbol{J}_{eO}(\boldsymbol{q}) \end{bmatrix} \dot{\boldsymbol{q}} = \boldsymbol{J}_e(\boldsymbol{q}) \dot{\boldsymbol{q}}
\end{aligned} 
\qquad (2.5) 
$$

where $\boldsymbol{J}_e(\boldsymbol{q}) \in \mathbb{R}^{6 \times n}$ is the configuration dependent geometric Jacobian matrix of the manipulator.

It is sometimes convenient to express the end-effector's velocity with respect to another frame $u$ of the kinematic chain instead of the base frame; it is often the case for the end-effector frame ($u = e$). If the rotation matrix between the base and the desired frame ($u$) is known ($f\boldsymbol{R}_u$) and using the relationship between velocities expressed in two different frames

$$ 
\begin{bmatrix} u \boldsymbol{v}_e \\ u \boldsymbol{\omega}_e \end{bmatrix} = \begin{bmatrix} f \boldsymbol{R}_u^\top & \boldsymbol{O} \\ \boldsymbol{O} & f \boldsymbol{R}_u^\top \end{bmatrix} \begin{bmatrix} f \boldsymbol{v}_e \\ f \boldsymbol{\omega}_e \end{bmatrix} = f \boldsymbol{R}_u^\top \begin{bmatrix} f \boldsymbol{v}_e \\ f \boldsymbol{\omega}_e \end{bmatrix} 
\qquad (2.6) 
$$

where $\boldsymbol{O}$ is the matrix of all zero elements of proper dimension. Exploiting this relationship, one can obtain the end-effector linear and angular velocities in its own frame from the joint velocities as

$$ 
\begin{aligned}
e \boldsymbol{v}_e = \begin{bmatrix} e \boldsymbol{v}_e \\ e \boldsymbol{\omega}_e \end{bmatrix} = f \boldsymbol{R}_e^\top \boldsymbol{J}_e(\boldsymbol{q}) \dot{\boldsymbol{q}} = e \boldsymbol{J}_e(\boldsymbol{q}) \dot{\boldsymbol{q}}
\end{aligned} 
\qquad (2.7) 
$$

The method for computing the geometric Jacobian essentially follows a geometric procedure that identifies the individual contributions of each joint velocity to the linear and angular velocity components of the end-effector.
Another possibility to compute the Jacobian consists in deriving it directly from the direct geometric model functions ($\mathcal{K}(\mathbf{q})$) as

$$ 
\begin{aligned}
{}^f\dot{\mathbf{p}}_e &= \frac{\partial {}^f\mathbf{p}_e}{\partial \mathbf{q}} \dot{\mathbf{q}} = \mathbf{J}_{eP}(\mathbf{q}) \dot{\mathbf{q}} \\
{}^f\dot{\phi}_e &= \frac{\partial {}^f\phi_e}{\partial \mathbf{q}} \dot{\mathbf{q}} = \mathbf{J}_{e\phi}(\mathbf{q}) \dot{\mathbf{q}}
\end{aligned}
\qquad (2.8) $$

the linear velocity of the end-effector can be obtained as the time derivative of its position vector in the base frame (${}^f\dot{\mathbf{p}}_e$), while for the orientation, the time derivative of the minimal orientation representation does not coincide, in general, with the vector of angular velocities (${}^f\dot{\phi}_e \neq {}^f\omega_e$). Furthermore, the computation of the Jacobian $\mathbf{J}_{e\phi}(\mathbf{q}) = \frac{\partial {}^f\phi_e}{\partial \mathbf{q}}$ is not straightforward since the function ${}^f\phi_e$ is not usually available in explicit form and its computation requires to pass through the elements of the relative rotation matrix (${}^f\mathbf{R}_e({}^f\phi_e)$). Formally deriving the direct geometric model equation (2.1) one yields to the following differential kinematic equation

$$ \dot{\mathbf{x}}_e = \begin{bmatrix} {}^f\dot{\mathbf{p}}_e \\ {}^f\dot{\phi}_e \end{bmatrix} = \begin{bmatrix} \mathbf{J}_{eP}(\mathbf{q}) \\ \mathbf{J}_{e\phi}(\mathbf{q}) \end{bmatrix} \dot{\mathbf{q}} = \mathbf{J}_A(\mathbf{q}) \dot{\mathbf{q}} \qquad (2.9) $$

where

$$ \mathbf{J}_A(\mathbf{q}) = \frac{\partial \mathcal{K}(\mathbf{q})}{\partial \mathbf{q}} \qquad (2.10) $$

is called the analytic Jacobian, which results to be different from the geometric Jacobian $\mathbf{J}_e(\mathbf{q})$ computed using the geometric procedure. Exploiting the existing relationship between angular velocity and time derivative of a minimal orientation representation

$$ {}^f\omega_e = \Gamma({}^f\phi_e) {}^f\dot{\phi}_e \qquad (2.11) $$

where $\Gamma({}^f\phi_e)$ is a suitable representation dependent transformation matrix, it is possible to find the link between the two Jacobians. Taking into account (2.5) and (2.9)

$$ {}^f\mathbf{v}_e = \begin{bmatrix} \mathbb{I}_3 & \mathbf{O} \\ \mathbf{O} & \Gamma({}^f\phi_e) \end{bmatrix} \dot{\mathbf{x}}_e = \Gamma_A({}^f\phi_e) \dot{\mathbf{x}}_e = \Gamma_A({}^f\phi_e) \mathbf{J}_A(\mathbf{q}) \dot{\mathbf{q}} \qquad (2.12) $$

where $\mathbb{I}_3$ is the 3 × 3 identity matrix. We finally obtain

$$ \mathbf{J}_e(\mathbf{q}) = \Gamma_A({}^f\phi_e) \mathbf{J}_A(\mathbf{q}) \qquad (2.13) $$

### 2.2.1.1 Second-order differential kinematics

Kinematic inversion algorithms are first-order algorithms in the sense that they allow the inversion of a trajectory specified in terms of end-effector position and
velocity into the equivalent position and velocity of the joints. As we will see later, inverting a trajectory specified in terms of position, velocity, and acceleration in operational space may be necessary for control purposes. The manipulator, on the other hand, is a naturally second-order mechanical system, as the dynamic model will show. The derivation of the differential kinematic equation (2.5) allows to write

$$f \dot{\mathbf{v}}_e = \dot{\mathbf{J}}_e(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{J}_e(\mathbf{q})\ddot{\mathbf{q}} \quad (2.14)$$

while the derivation of equation (2.9) yields

$$\ddot{x}_e = \dot{\mathbf{J}}_A(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{J}_A(\mathbf{q})\ddot{\mathbf{q}} \quad (2.15)$$

### 2.2.2 Statics

The goal of this subsection is to find the relationship between the forces ($f \mathbf{f}_e \in \mathbb{R}^3$) and moments ($f \boldsymbol{\mu}_e \in \mathbb{R}^3$) acting on the end-effector (the wrench $\mathbf{h}_e = [f \mathbf{f}_e^\top f \boldsymbol{\mu}_e^\top]^\top$) and the forces/torques ($\boldsymbol{\tau} \in \mathbb{R}^n$) at the joints (force for prismatic, torque for revolute joints).

The considered manipulators are mechanical system with time-independent holonomic constraints whose configurations only depend on the joint variables $\mathbf{q}$ and not explicitly on time. This means that the virtual displacements (indicated with $\delta$) of the structure coincide with the elementary displacements (indicated with $d$). Applying therefore the Virtual Work Principle we can obtain the desired relation.

Let us consider the elementary work done by the two force systems. On one hand, one has the force/torques acting on the joints, while in the other hand, one has to split the contribution to the work performed by the forces and the moments at the origin of the end-effector frame. The elementary works are then:

$$\begin{aligned} dW_{\tau} &= \boldsymbol{\tau}^{\top} d\mathbf{q} \\ dW_{h} &= f \mathbf{f}_{e}^{\top} d^{f} \mathbf{p}_{e} + f \boldsymbol{\mu}_{e}^{\top} f \boldsymbol{\omega}_{e} dt \end{aligned} \quad (2.16)$$

where $dW_\tau$ and $dW_h$ are respectively the elementary work performed by the joint torques $\boldsymbol{\tau}$ and the wrench $\mathbf{h}_e$. $d^f \mathbf{p}_e$ and $f \boldsymbol{\omega}_e dt^1$ are the linear and the angular displacements. Taking into account the differential kinematic equation (2.5)

$$dW_h = f \mathbf{f}_e^\top \mathbf{J}_{e_P}(\mathbf{q}) d\mathbf{q} + f \boldsymbol{\mu}_e^\top \mathbf{J}_{e_O}(\mathbf{q}) d\mathbf{q} = \mathbf{h}_e^\top \mathbf{J}_e(\mathbf{q}) d\mathbf{q} \quad (2.17)$$

<footnote n="1">¹Due to the integrability problems related to the angular displacement it has been indicated by $f \boldsymbol{\omega}_e dt$.</footnote>
Because the virtual and elementary displacements coincide, the virtual work equals the elementary work, then

$$ 
\begin{aligned}
\delta W_{\tau} &= \tau^{\top} \delta \boldsymbol{q} \\
\delta W_{h} &= \boldsymbol{h}_{e}^{\top} \boldsymbol{J}_{e}(\boldsymbol{q}) \delta \boldsymbol{q}
\end{aligned}
\qquad (2.18) $$

and for the Virtual Work Principle, the manipulator is in static equilibrium if and only if

$$ \delta W_{\tau} = \delta W_{h} \quad \forall \delta \boldsymbol{q} \qquad (2.19) $$

yielding to the well-known relation

$$ \tau = \boldsymbol{J}_{e}^{\top}(\boldsymbol{q})\boldsymbol{h}_{e} \qquad (2.20) $$

which by virtue of the relation (2.13) can also be expressed as

$$ \tau = \boldsymbol{J}_{e}^{\top}(\boldsymbol{q})\boldsymbol{h}_{e} = (\boldsymbol{\Gamma}_{A}(^{f}\boldsymbol{\phi}_{e})\boldsymbol{J}_{A}(\boldsymbol{q}))^{\top}\boldsymbol{h}_{e} = \boldsymbol{J}_{A}(\boldsymbol{q})^{\top}\boldsymbol{h}_{A} \qquad (2.21) $$

where of course

$$ \boldsymbol{h}_{A} = \boldsymbol{\Gamma}_{A}(^{f}\boldsymbol{\phi}_{e})^{\top}\boldsymbol{h}_{e} \qquad (2.22) $$

### 2.2.3 Twist and Wrench transformations

<figure><img src="image_8.png" /><figcaption>**Figure 2.2** – Linear and angular velocity representations in different coordinate frames on the same rigid-body. The Wrench-transformation is used to express the measured contact wrench on the Force/Torque sensor frame ($\Sigma_A$) into the end-effector frame ($\Sigma_B$).</figcaption></figure>

Let consider a fixed frame of reference $\Sigma_w$ and a rigid-body in motion with respect to such frame. Let $\Sigma_A$ and $\Sigma_B$ be two coordinate frames attached to the
rigid-body as shown in Figure 2.2. The linear and angular velocity relationship between frames $\Sigma_A$ and $\Sigma_B$ with respect to the fixed frame $\Sigma_w$ are given by:

$$ \begin{aligned} w\omega_{w,B} &= w\omega_{w,A} \\ w\dot{\mathbf{p}}_{w,B} &= w\dot{\mathbf{p}}_{w,A} + w\omega_{w,A} \times w\mathbf{p}_{A,B} \end{aligned} \qquad (2.23) $$

which, exploiting the matrix form of the cross-product, can be written in compact form as

$$ \begin{bmatrix} w \dot{\mathbf{p}}_{w,B} \\ w \omega_{w,B} \end{bmatrix} = \begin{bmatrix} \mathbb{I} & -[w \mathbf{p}_{A,B}]_\times \\ \mathbf{O} & \mathbb{I} \end{bmatrix} \begin{bmatrix} w \dot{\mathbf{p}}_{w,A} \\ w \omega_{w,A} \end{bmatrix} \qquad (2.24) $$

being $[\cdot]_\times$ the 3 × 3 skew-symmetric matrix operator. If the vectors are instead expressed in their own frames

$$ \begin{aligned} w \mathbf{p}_{A,B} &= w \mathbf{R}_A A \mathbf{p}_{A,B} \\ w \dot{\mathbf{p}}_{w,A} &= w \mathbf{R}_A A \dot{\mathbf{p}}_{w,A} \\ w \dot{\mathbf{p}}_{w,B} &= w \mathbf{R}_B B \dot{\mathbf{p}}_{w,B} \\ w \omega_{w,A} &= w \mathbf{R}_A A \omega_{w,A} \\ w \omega_{w,B} &= w \mathbf{R}_B B \omega_{w,B} \\ w \mathbf{R}_A A \omega_{w,B} &= w \mathbf{R}_B B \omega_{w,B} \end{aligned} $$

and taking into account equation (2.24) and the properties of the skew-symmetric matrix operator [13], one yields the *Twist-transformation*

$$ {}^{B}\mathbf{v}_{w,B} = \begin{bmatrix} {}^{B}\dot{\mathbf{p}}_{w,B} \\ {}^{B}\omega_{w,B} \end{bmatrix} = \begin{bmatrix} {}^{B}\mathbf{R}_A & -{}^{B}\mathbf{R}_A [A\mathbf{p}_{A,B}]_\times \\ \mathbf{O} & {}^{B}\mathbf{R}_A \end{bmatrix} \begin{bmatrix} {}^{A}\dot{\mathbf{p}}_{w,A} \\ {}^{A}\omega_{w,A} \end{bmatrix} = {}^{B}\mathbb{T}_A A \mathbf{v}_{w,A} \qquad (2.25) $$

being ${}^{B}\mathbb{T}_A$ the *Twist-transformation* matrix, which is a proper Jacobian matrix that expresses the linear and angular velocities from a reference frame into another. Due to the *kineto-static* duality [13], one can directly obtain the *Wrench-transformation* as

$$ {}^{A}\mathbf{h}_{A} = \begin{bmatrix} {}^{A}\mathbf{f}_{A} \\ {}^{A}\boldsymbol{\mu}_{A} \end{bmatrix} = \begin{bmatrix} {}^{A}\mathbf{R}_{B} & \mathbf{O} \\ [{}^{A}\mathbf{p}_{A,B}]_{\times}^{A}\mathbf{R}_{B} & {}^{A}\mathbf{R}_{B} \end{bmatrix} \begin{bmatrix} {}^{B}\mathbf{f}_{B} \\ {}^{B}\boldsymbol{\mu}_{B} \end{bmatrix} = {}^{B}\mathbb{T}_{A}^{\top} {}^{B}\mathbf{h}_{B} \qquad (2.26) $$

being the *Wrench-transformation* matrix ${}^{A}\mathbb{F}_{B} = {}^{B}\mathbb{T}_{A}^{\top}$.

## 2.3 Dynamic model

The dynamic model of a robotic arm can be written using the *Lagrange* formulation in the joint space as described in [13]:

$$ \mathbf{B}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{F}_v\dot{\mathbf{q}} + \mathbf{F}_s\mathrm{sgn}(\dot{\mathbf{q}}) + \mathbf{g}(\mathbf{q}) + \mathbf{J}_e^{\top}(\mathbf{q})\mathbf{h}_e = \boldsymbol{\tau} \qquad (2.27) $$

where $\dot{\mathbf{q}}, \ddot{\mathbf{q}} \in \mathbb{R}^n$ are respectively the generalized joint velocities and accelerations. $\mathbf{B}(\mathbf{q}) \in \mathbb{R}^{n \times n}$ is the symmetric and positive definite inertia matrix,
$C(q, \dot{q}) \in \mathbb{R}^{n \times n}$ is the matrix of centrifugal and Coriolis effects, $g(q) \in \mathbb{R}^n$ is the configuration dependent vector of gravitational forces, $\tau_e = J_e^\top h_e \in \mathbb{R}^n$ is the joint vector corresponding to the external wrench $h_e \in \mathbb{R}^6$ acting on the end-effector frame, $\tau \in \mathbb{R}^n$ is the vector of joint actuation torques. Finally, $F_v \dot{q} \in \mathbb{R}^n$ and $F_s sgn(\dot{q}) \in \mathbb{R}^n$ represents the joint vectors of viscous and coulomb friction.

<figure><img src="image_9.png" /><figcaption>Figure 2.3 – Kinematic characterization of the i-th link for the Lagrangian formulation.</figcaption></figure>

### 2.3.1 Parameter identification

The dynamic model in the form (2.27) includes typically nonlinear functions of $q$, $\dot{q}$, $\ddot{q}$ and the dynamic parameters described here in detail further. For each link $l_i$, $i = 1, \dots, n$, let $m_i$ be its mass and let

$$ {}^i \boldsymbol{r}_{i,ci} = \begin{bmatrix} c_{ix} \\ c_{iy} \\ c_{iz} \end{bmatrix}, \quad {}^i \boldsymbol{J}_{l_i} = \begin{bmatrix} J_{ixx} & J_{ixy} & J_{ixz} \\ J_{ixy} & J_{iyy} & J_{iyz} \\ J_{ixz} & J_{iyz} & J_{izz} \end{bmatrix}, \qquad (2.28) $$

be the position of the center of mass and the symmetric inertia tensor with respect to the i-th link frame, respectively, see Figure 2.3.

At this stage, if we collect the dynamic parameters of all the robot links in the three vectors

$$ \begin{aligned} \boldsymbol{p}_1 &= [m_1 \quad \dots \quad m_n]^\top, \\ \boldsymbol{p}_2 &= [c_{1x}m_1 \quad c_{1y}m_1 \quad c_{1z}m_1 \quad \dots \quad c_{nx}m_n \quad c_{ny}m_n \quad c_{nz}m_n]^\top, \qquad (2.29) \\ \boldsymbol{p}_3 &= [\mathcal{J}_1^\top \quad \dots \quad \mathcal{J}_n^\top]^\top, \end{aligned} $$
with $\mathbf{p}_1 \in \mathbb{R}^n$, $\mathbf{p}_2 \in \mathbb{R}^{3n}$, $\mathbf{p}_3 \in \mathbb{R}^{6n}$ and

$$ \mathcal{J}_i = [ \mathcal{J}_{ixx} \ \mathcal{J}_{ixy} \ \mathcal{J}_{ixz} \ \mathcal{J}_{iyy} \ \mathcal{J}_{iyz} \ \mathcal{J}_{izz} ]^\top, \quad (2.30) $$

it is possible to rearrange (2.27) as

$$ \mathbf{Y}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) \pi(\mathbf{p}_1, \mathbf{p}_2, \mathbf{p}_3) = \tau, \quad (2.31) $$

where the vector $\pi(\mathbf{p}_1, \mathbf{p}_2, \mathbf{p}_3) = [ \mathbf{p}_1^\top \ \mathbf{p}_2^\top \ \mathbf{p}_3^\top ]^\top \in \mathbb{R}^p$ [20]. Then, $\pi$ appears linearly in the dynamic model (2.31), multiplied by the regressor matrix $\mathbf{Y}$ of known time-varying functions.

The dynamic identification procedure is performed by collecting $M \gg n p$ joint torque samples as well as $M$ joint position samples, while the joint velocity and the acceleration are computed by off-line differentiation. For each numerical sample $(\tau_k, \mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k)$, with $k = 1, \dots, M$, we have

$$ \mathbf{Y}_k(\mathbf{q}_k, \dot{\mathbf{q}}_k, \ddot{\mathbf{q}}_k)\pi = \tau_k. \quad (2.32) $$

By stacking these quantities in vectors and matrices, one has

$$ \bar{\mathbf{Y}}\pi = \bar{\tau}, \quad (2.33) $$

with $\bar{\tau} \in \mathbb{R}^{Mn}$ and $\bar{\mathbf{Y}} \in \mathbb{R}^{Mn \times p}$. According to [21], we can prune the stacked regressor $\bar{\mathbf{Y}}$ so as to obtain a matrix with full column rank $\bar{\mathbf{Y}}_R$, and then identify the dynamic coefficients by solving an ordinary least-squares (OLS) problem via pseudoinversion

$$ \hat{\pi}_R = \bar{\mathbf{Y}}_R^\dagger \bar{\tau}. \quad (2.34) $$

With the solution $\hat{\pi}_R \in \mathbb{R}^p$ of regrouped dynamic parameters, i.e., the dynamic coefficients, we can provide a joint torque estimate as

$$ \hat{\tau} = \mathbf{Y}_R(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}})\hat{\pi}_R \quad (2.35) $$

for validation on any new motion $\mathbf{q}(t)$. Finally, following [20], one can extract from the identified vector $\hat{\pi}_R$ a feasible set of dynamic parameters $\hat{\mathbf{p}} = (\hat{\mathbf{p}}_1, \hat{\mathbf{p}}_2, \hat{\mathbf{p}}_3)$ (not necessarily the true ones) such that $\pi_R(\hat{\mathbf{p}}_1, \hat{\mathbf{p}}_2, \hat{\mathbf{p}}_3) = \hat{\pi}_R$ and the upper/lower bounds on the components of $\mathbf{p}_i$, $i = 1, 2, 3$, are also satisfied. However, the triangle inequality constraint of inertia tensors is not taken into account in [20], as instead done in Section 7.4.

Details on the physical consistency and the triangle inequality are provided in the next subsection.
### 2.3.2 Physical consistency of the dynamic parameters

The obtained estimation of the dynamic coefficients vector $\hat{\pi}_R$ might be possibly physically inconsistent (e.g., a negative link mass), and this can be caused, for instance, by modeling errors or by noisy measurements. Recent works [22–24] highlighted these physical constraints and provided frameworks to consider them during the identification phase, by solving linear constrained optimization problems using the following cost function:

$$ \min_{\pi} f(\pi) = \|\bar{Y}\pi - \bar{\tau}\|. \qquad (2.36) $$

Physical constraints regard the mass of each link, which has to be positive, and the barycentric inertia tensor of each link, which has to be positive definite. That is, for each link $l_i$ of the manipulator, one has:

$$ m_i > 0 \qquad (2.37) $$

and

$$ \mathbf{I}_{\ell_i} = \begin{bmatrix} I_{ixx} & I_{ixy} & I_{ixz} \\ I_{ixy} & I_{iyy} & I_{iyz} \\ I_{ixz} & I_{iyz} & I_{izz} \end{bmatrix} > 0, \qquad (2.38) $$

where $\mathbf{I}_{\ell_i}$ is the inertia tensor of link $l_i$ with respect to its center of mass. Moreover, it is always possible to express the barycentric inertia tensor in a diagonal form, exploiting a particular rotation matrix $\bar{\mathbf{R}}_i$, such as

$$ \mathbf{I}_{\ell_i} = \bar{\mathbf{R}}_i \bar{\mathbf{I}}_{\ell_i} \bar{\mathbf{R}}_i^T, \qquad (2.39) $$

where $\bar{\mathbf{I}}_{\ell_i}$ is the diagonal inertia tensor. Since the diagonal elements ($\bar{\mathbf{I}}_{i,x}$, $\bar{\mathbf{I}}_{i,y}$, $\bar{\mathbf{I}}_{i,z}$) of $\bar{\mathbf{I}}_{\ell_i}$ are also the eigenvalues of $\mathbf{I}_{\ell_i}$, condition (2.38) can be rewritten as

$$ \bar{\mathbf{I}}_{i,x} > 0, \quad \bar{\mathbf{I}}_{i,y} > 0, \quad \bar{\mathbf{I}}_{i,z} > 0. \qquad (2.40) $$

These three inequalities are however included in the following triangle inequality:

$$ \left\{ \begin{array}{l} \bar{\mathbf{I}}_{i,x} + \bar{\mathbf{I}}_{i,y} > \bar{\mathbf{I}}_{i,z} \\ \bar{\mathbf{I}}_{i,y} + \bar{\mathbf{I}}_{i,z} > \bar{\mathbf{I}}_{i,x} \\ \bar{\mathbf{I}}_{i,z} + \bar{\mathbf{I}}_{i,x} > \bar{\mathbf{I}}_{i,y} \end{array} \right. \qquad (2.41) $$

which, with simple manipulations [24], leads to the following condition on $\mathbf{I}_{\ell_i}$:

$$ \frac{\mathrm{tr}(\mathbf{I}_{\ell_i})}{2} - \lambda_{\max}(\mathbf{I}_{\ell_i}) > 0, \qquad (2.42) $$

since $\bar{\mathbf{I}}_{i,x} + \bar{\mathbf{I}}_{i,y} + \bar{\mathbf{I}}_{i,z} = \mathrm{tr}(\mathbf{I}_{\ell_i})$ and denoting with $\mathrm{tr}(\mathbf{I}_{\ell_i})$ and $\lambda_{\max}(\mathbf{I}_{\ell_i})$, respectively, the trace and the maximum eigenvalue of the inertia tensor $\mathbf{I}_{\ell_i}$.

Therefore, in order to guarantee physical consistency of the dynamic parameters, conditions (2.37) and (2.42) must be satisfied for each link and are therefore added as constraints to the optimization problem (see Section 7.4).
### 2.3.3 Dynamic model in operational space

To develop a dynamic model in operational space capable of describing the behavior of redundant and non-redundant manipulation structures, the dynamic model in joint space should be used as a starting point for the description, as its application is quite general. To that end, if one solves equation (2.27) in terms of joint accelerations while ignoring for simplicity the joint friction torques, one obtains

$$ \ddot{q} = -B(q)^{-1}C(q, \dot{q})\dot{q} - B(q)^{-1}g(q) + B(q)^{-1}J_e^\top(q)(\gamma_e - h_e) \quad (2.43) $$

in which in virtue of equation (2.20), the contribution to the joint actuation torques has been expressed in terms of the equivalent forces at the end-effector as $\tau = J_e^\top(q)\gamma_e$. We recall that the term $h_e$ expresses the contribution to the end-effector forces due to contact with the environment while $\gamma_e$ is the contribution to the end-effector forces due to the motor actuation torques.

The second-order differential kinematic equation (2.15) expresses the relationship between joint and operational space accelerations through the robot analytic Jacobian $J_A$ while in the Lagrangian equation of motion (2.27) appears the geometric Jacobian $J_e$. Similarly to relation (2.22), it is possible to write

$$ \gamma_A = \Gamma_A(f\phi_e)^\top\gamma_e \quad (2.44) $$

Replacing (2.43) into the second-order differential kinematic equation (2.15) one obtains (omitting the dependencies on $q$ and $\dot{q}$ for the sake of notation simplicity)

$$ \ddot{x}_e = \dot{J}_A \dot{q} - J_A B^{-1} C \dot{q} - J_A B^{-1} g + J_A B^{-1} J_A^\top (\gamma_A - h_A) \quad (2.45) $$

that rearranged and naming the quantities ($\dot{q} = J_A^\dagger \dot{x}_e$)

$$ B_A = (J_A B^{-1} J_A^\top)^{-1} \quad (2.46a) $$

$$ C_A \dot{x}_e = B_A (J_A B^{-1} C - \dot{J}_A) J_A^\dagger \dot{x}_e \quad (2.46b) $$

$$ g_A = B_A J_A B^{-1} g \quad (2.46c) $$

yields to

$$ B_A(x_e) \ddot{x}_e + C_A(x_e, \dot{x}_e) \dot{x}_e + g_A(x_e) + h_A = \gamma_A \quad (2.47) $$

The obtained dynamic model in operational space is formally analogous to the one in joint space.
## 2.4 Force Control Schemes

The ability to manage the physical contact between a robot and its environment is a critical necessity for the accomplishment of a manipulation task. Pure motion control is insufficient given the inevitable modelling errors and uncertainties that can result in an increase in contact force, ultimately leading to unstable behaviour during interaction, especially in rigid environments. Feedback and force control are necessary to provide robust and flexible behaviour of the robotic system in unstructured environments and to have a safe and reliable operation in the presence of human operators.

In this section, we describe some of the most common force control methods in the literature, that have also been used as base controllers for the integrated vision-force methods we will discuss in Chapter 4.

### 2.4.1 Hybrid position/force control

Typically, a robotic task is separated into two main phases: a large motion, in which only position control is used, and fine quasi-static motion, in which force control is the primary concern. However, certain tasks may necessitate precise hybrid control of both velocity and force along a dynamic trajectory.

Following Mason's task description [15], Raibert and Craig [14] proposed a technique capable of controlling either the velocity or the force exchanged by each direction axis of a manipulator in compliant motion with the environment. The essential idea behind their control technique is an architectural concept conceived to disregard position errors along force-controlled axes and force errors along position-controlled axes. On the basis of the relative error, a typical PD/PID force or position controller is designed individually for each axis. This error is calculated using kinematic transformations then, selection matrices ($S$ in Figure 2.4), a key element of this controller to keep force- and position-controlled directions separated, avoiding the arise of inconsistencies at the actuation level. In particular, given the task geometry and the constraints imposed by the environment, a subset of the controlled axis will be position/velocity controlled while the remaining ones will be force-controlled (those along the constraints imposed by the environment).

With reference to Figure 2.4, the motion of the workpiece which is rigidly handled by the manipulator's gripper, is constrained by the flat surface on which it slides; linear velocity along the end-effector's $z$-axis as well as angular velocities about the $x$- and $y$-axis are constrained by the task geometry. Moreover, forces
<figure><img src="image_10.png" /><figcaption>**Figure 2.4** – Hybrid force/motion control scheme while executing a cube in plane sliding task. $S = S_v$ is the matrix that selects the motion controlled directions, while $S_f = I - S$ selects the force controlled ones. The end-effector's current and reference pose are indicated with symbols $\mathbf{x}_e$ and $\dot{\mathbf{x}}_e$ respectively while $\dot{\mathbf{x}}_e$ and $\dot{\mathbf{x}}_e^d$ represent its current and reference velocities. The measured and desired end-effector's external wrench are instead indicated with symbols $^e\mathbf{h}_e$ and $^e\mathbf{h}_e^d$. The position (2.51) and force (2.54) control laws produce the joint torque contributions that are summed up in $\boldsymbol{\tau}$ to servo the robot along the free motion and constrained Cartesian directions respectively.</figcaption></figure>

along $x-$ and $y$-axis and moments about the $z$-axis cannot be arbitrarily imposed; all those are called *natural constraints*. Conversely, the manipulator can be commanded to move along $x-$ and $y$-axis and about $z$-axis while arbitrary moments about $x-$ and $y$-axis and forces along the $z$-axis can be required to the controller. The latter are named *Natural constraints* and are used to determine the selection matrix for the velocity subspace $S_v$. The selection matrix of the force subspace is quickly obtained as $S_f = I - S_v$. For this particular example, selection matrices are:

$$S_v = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \qquad S_f = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} \qquad (2.48)$$
The joint torque controller has the following expression

$$ \tau = \chi(S_v, \Delta x, \Delta \dot{x}) + \Psi(S_f, \Delta f) \quad (2.49) $$

$$ \chi(S_v, \Delta x, \Delta \dot{x}) = K_{p_P} J_A^{-1} \Delta x + K_{p_D} J_A^{-1} \Delta \dot{x} + K_{p_I} \int J_A^{-1} \Delta x \quad (2.50) $$

$$ \Delta x = S_v (f x_e^d - K(q)) \quad (2.51) $$

$$ \Delta \dot{x} = S_v (f \dot{x}_e^d - J_A(q) \dot{q}) \quad (2.52) $$

$$ \Psi(S_f, \Delta f) = J_e^\top S_f e h_e^d + K_{f_P} J_e^\top \Delta f + K_{f_I} \int J_e^\top \Delta f \quad (2.53) $$

$$ \Delta f = S_f (e h_e^d - e \mathbb{F}_{F/T}^{F/T} h_{F/T}) \quad (2.54) $$

where $J_A^{-1} \Delta x$ is a differential approximation valid only for small $\Delta x$. Otherwise it can be seen as the inverse kinematic problem which provides as solution the joint space error $q_e$; $\chi(S_v, \Delta x, \Delta \dot{x})$ implements a PID controller on the positional error $\Delta x$ while $\Psi(S_f, \Delta f)$ implements a force reference feedforward plus a PI on the wrench error $\Delta f$. Both force and position contributions to the joint torques are “filtered out” by the relative selection matrices.

Several improvements to the presented hybrid control of manipulators have been proposed since its formulation in 1981 for: compensating the interaction effects due to dynamic coupling like in [11] or in [25] which also preserves the dynamic orthogonality between force and velocity; for accounting for the followed geometric curvature of the object surface [26, 27]; a robust version to dynamic parameters is given in [28] while a generalization for controlling robots in contact with dynamic environments is given in [29]. Kinematic stability issues due to the inverse of the manipulator Jacobian and selection matrices are treated in [30] and [31] respectively.

The presented hybrid position/force controller generates the torque commands for the joints; nevertheless, following the same reasoning, a controller that generates the joint velocities instead of the torques can be synthesized. What matters is the preservation of the orthogonal subspaces through the use of the selection matrices.

## 2.4.2 Impedance control

The ratio of force output to motion input is known as mechanical impedance; regulating the impedance of a mechanism means adjusting the force of resistance to external motion imposed by the environment. Impedance control seeks to implement a dynamic relationship between manipulator variables such as end-effector position and wrench, rather than simply controlling these variables separately. This concept was originally applied to manipulators by Hogan [12] in 1985. The
strategy that has been chosen to deal with the inertial behaviour of the manipulator is to “hide” the underlying non-linear inertial dynamics of the manipulator and impose a simpler one; that of a rigid body.

A control technique for such mechanical systems, which aims at linearizing and decoupling the manipulator dynamics via feedback, is the well-known inverse dynamics control. Nonlinearities such as Coriolis and centrifugal forces as well as gravitational forces, can be cancelled by adding those terms in the control input, while decoupling is achieved through the inertia matrix. The following development follows more closely the development in [13].

From the manipulator's equation of motion (2.27), we want to synthesize an inverse dynamics controller in operational space that minimizes the error $\tilde{x}_e = x_e^d - x_e$. Choosing a joint space control input

$$u = B(q)\alpha + N(q, \dot{q}) \quad (2.55)$$

where $N(q, \dot{q}) = C(q, \dot{q})\dot{q} + F_v\dot{q} + F_s\text{sgn}(\dot{q}) + g(q)$ and $\alpha$ is the new resolved joint space acceleration controller to be properly designed. The manipulator (2.27) under the action of such controller, is described by

$$\ddot{q} = \alpha - B^{-1}J_e^T(q)h_e \quad (2.56)$$

which, exploiting the second-order differential kinematic equation (2.15) to link joint and end-effector accelerations, suggest the expression of $\alpha$

$$\alpha = J_A(q)^{-1}M_d^{-1}(M_d\ddot{x}_e^d + D\dot{\tilde{x}}_e + K\tilde{x}_e - M_d\dot{J}_A(q, \dot{q})\dot{q}) \quad (2.57)$$

which includes a PD with acceleration feedforward in operational space, being $M_d$, $D$ and $K$ suitable positive definite matrices. Replacing equation (2.57) into (2.56) and accounting for the second-order differential equation, one obtains the closed-loop system equation

$$M_d\ddot{\tilde{x}}_e + D\dot{\tilde{x}}_e + K\tilde{x}_e = M_d B_A^{-1}(q)h_A \quad (2.58)$$

where $B_A(q) = J_A(q)^{-\top}B(q)J_A(q)^{-1}$ is the inertia matrix of the manipulator in operational space (see Section 2.3.3). The closed-loop system equation defines a generalized mechanical impedance between the displacement vector $\tilde{x}_e$ and the vector of resultant forces $M_d B_A^{-1}(q)h_A$ in the operational space. This impedance is equivalent to a mechanical system characterized by the matrices of inertia ($M_d$), damping ($D$) and stiffness ($K$).

The presence of the configuration dependent inertia matrix $B_A(q)^{-1}$ keeps the system dynamically coupled during the interaction. Using a Force/Torque
sensor, it is possible to measure the contact wrench ($h_e$) and use it to preserve the linearity and decoupled properties of the system. Adding it then in the input control law

$$u = B(q)\alpha + N(q, \dot{q}) + J_e^\top h_e, \qquad (2.59)$$

fully compensates for the external forces, making the manipulator infinitely stiff with respect to those forces; then with

$$\alpha = J_A(q)^{-1} M_d^{-1} (M_d \ddot{x}_e^d + D \dot{\tilde{x}}_e + K \tilde{x}_e - M_d \dot{J}_A(q, \dot{q}) \dot{q} - h_A) \quad (2.60)$$

in which $-J_A(q)^{-1}M_d^{-1}h_A$ has been added to endow the system with a linear impedance behaviour with respect to the vector $h_A$, as the closed loop equation shows

$$M_d \ddot{\tilde{x}}_e + D \dot{\tilde{x}}_e + K \tilde{x}_e = h_A \qquad (2.61)$$

The control scheme of the impedance control in interaction with the environment is depicted in Figure 2.5.

<figure><img src="image_11.png" /><figcaption>**Figure 2.5** – Impedance control scheme. The simplest version of the controller (eq. (2.57)) does not require any Force/Torque measurement. This scheme relates to the controller eq. (2.60) and inverse dynamics eq. (2.59).</figcaption></figure>

At equilibrium, i.e., when the manipulator is at rest ($q = \dot{q} = 0$), the position of the end-effector is given by

$$x_e = x_e^d - K^{-1}h_A \qquad (2.62)$$

When the manipulator is not in contact with the environment ($h_A = 0$) the actual position of the end-effector reaches the desired one ($x_e = x_e^d$). When in
contact with the environment, the positioning error is proportional to the inverse of the chosen stiffness $K$ and the equivalent contact wrench $h_A$. It should be noted that, with respect to the contact wrench $h_e$, the impedance is dependent on the orientation of the end-effector due to the presence of the matrix $\Gamma_A(^f\phi_e)$ (see (2.22)), making the choice of the impedance parameters difficult.

To avoid this problem it is sufficient to redesign the control input $\alpha$ with respect to a different error vector in operational space

$$ \tilde{\mathbf{x}} = - \begin{bmatrix} ^d\mathbf{p}_{d,e} \\ ^d\phi_{d,e} \end{bmatrix} \qquad (2.63) $$

where $^d\mathbf{p}_{d,e}$ is the position error between the origin of the current and desired end-effector frames and $^d\phi_{d,e}$ is the orientation vector extracted from the rotation matrix $^d\mathbf{R}_e(^d\phi_{d,e})$. For further details refer to [13].

### 2.4.3 Admittance control

The choice of the impedance parameters in the *impedance controller* determines its dynamic behaviour, both in free space and when in contact with the environment. Indeed if the interaction wrench is generated from contact with an environment described by a *mass-spring-damper*, the interacting system manipulator-environment is equivalent to a mechanical system made by the parallel of the two impedances, and the resulting closed-loop dynamics along the constrained directions will differ from those in free space. Since the dynamic behavior of the system is influenced by environmental rigidity, its stability is concerned as well.

Among the objectives to pursue when choosing the impedance parameters, there is the need to ensure a high disturbance rejection factor given the model uncertainties and the approximations induced in the computation of the inverse dynamics. This factor is proportional to the gain $K$ which also impacts the trajectory tracking performances. Indeed, the impedance controller in free space is nothing but an inverse dynamics position controller. It follows that tracking performance will be more degraded the lower this parameter is chosen. It is therefore evident that there is a trade-off between the tracking performance obtainable and the compliance assigned to the manipulator. The more rigid the manipulator is commanded to be, the better it will follow the reference trajectory and vice versa.

Separating the motion control and impedance control problems, as done in the control scheme reported in Figure 2.6, could be a solution to the aforementioned issues.

Let us suppose to have three reference frames, the current $\Sigma_c$, the desired $\Sigma_d$ and the compliant frame $\Sigma_{c*}$ and that the compliant frame is coincident with the
<figure><img src="image_12.png" /><figcaption>**Figure 2.6** – Admittance control scheme. It is composed by an internal “stiff” impedance controller (pose control eq. (2.57)) and inverse dynamics eq. (2.55)) and an external “impedance law” (eq. (2.66)).</figcaption></figure>

desired frame. The pose of those frames with respect to a fixed frame are given by

$$ {}^f\mathbf{T}_c = \begin{bmatrix} {}^f\mathbf{R}_c & {}^f\mathbf{p}_c \\ 0 & 0 \end{bmatrix}; {}^f\mathbf{T}_d = \begin{bmatrix} {}^f\mathbf{R}_d & {}^f\mathbf{p}_d \\ 0 & 0 \end{bmatrix}; {}^f\mathbf{T}_{c^*} = \begin{bmatrix} {}^f\mathbf{R}_{c^*} & {}^f\mathbf{p}_{c^*} \\ 0 & 0 \end{bmatrix} \quad (2.64) $$

Let us also suppose that there are two pose errors, $e_c$ between the current and the compliant frames and $\bar{e}_c$ between the desired and the compliant frame

$$ \begin{aligned} {}^{c^*}\mathbf{T}_c(e_c) &= {}^f\mathbf{T}_{c^*}^{-1} {}^f\mathbf{T}_c \rightarrow e_c = \begin{bmatrix} {}^{c^*}\mathbf{p}_{c^*,c} \\ {}^{c^*}\phi_{c^*,c} \end{bmatrix} \\ {}^{d^*}\mathbf{T}_{c^*}(\bar{e}_c) &= {}^f\mathbf{T}_d^{-1} {}^f\mathbf{T}_{c^*} \rightarrow \bar{e}_c = \begin{bmatrix} {}^{d^*}\mathbf{p}_{d,c^*} \\ {}^{d^*}\phi_{d,c^*} \end{bmatrix} \end{aligned} \quad (2.65) $$

The latter error is used as displacement vector in an impedance equation subjected to the action of a wrench $h_e$ which makes the compliant frame move according to the impedance law (2.66) (see Figure 2.7).

The admittance controller is based on this concept of compliant frame, which is a Cartesian reference frame that describes the ideal end-effector’s behavior as a result of impedance control. The linear and angular velocity and acceleration of the compliant frame are computed integrating impedance equations of the form

$$ M_t \ddot{\bar{e}}_c + D_t \dot{\bar{e}}_c + K_t \bar{e}_c = h_e \quad (2.66) $$

where $M_t$, $D_t$ and $K_t$ are the parameters of a mechanical impedance between the desired frame and the compliant frame.
<figure><img src="image_13.png" /><figcaption>**Figure 2.7** – Compliant frame concept: (a) Desired and Compliant frames are coincident, the Current frame tracks the compliant frame. (b) The system undergoes the action of an external force. (c) The compliant frame is linked to the desired one with a *mass-spring-damper* relationship.</figcaption></figure>

The position, velocity and acceleration of the compliant frame are then used as reference input to a “stiff” impedance controller, see Figure 2.6. The control parameters for both the motion and the compliance controller can be designed separately; ensuring high rejection factor and trajectory tracking performance for the motion controller on one hand, and imposing the desired interaction behaviour opportunely tuning the impedance parameters on the other. For stability purposes of the whole controlled system, the bandwidth of the motion controller must be greater than the one of the impedance.

**2.5 Conclusion**

In this introductory chapter, we examined the fundamental concepts of robot kinematics, its dynamic model, dynamic parameter identification, and force control of manipulators. These notions form the foundation of knowledge required to comprehend the content presented here. Furthermore, this chapter has allowed us to illustrate the fundamental concepts, terminology and symbols that will be used in the subsequent chapters.

On the other hand, as will be seen in Chapter 4, the motion/force control schemes discussed above will constitute the basic schemes upon which the main vision/force control schemes have been built, essentially replacing the motion loop with a vision-based control loop.

The focus of the next chapter is on vision control, the second essential brick on which this work is built on.
CHAPTER

3

Vision Control

Contents

<table><tr><td>3.1</td><td>Introduction</td><td>38</td></tr><tr><td>3.2</td><td>Camera model</td><td>39</td></tr><tr><td>3.3</td><td>Visual Servoing</td><td>40</td></tr><tr><td>3.3.1</td><td>IBVS: Image-Based Visual Servoing</td><td>42</td></tr><tr><td>3.3.2</td><td>PBVS: Pose-Based Visual Servoing</td><td>43</td></tr><tr><td>3.3.3</td><td>Stability analysis</td><td>45</td></tr><tr><td>3.3.4</td><td>Mounted camera</td><td>46</td></tr><tr><td>3.3.5</td><td>Feature trajectory tracking</td><td>47</td></tr><tr><td>3.3.6</td><td>Target tracking</td><td>47</td></tr><tr><td>3.4</td><td>Second-order Visual Servoing</td><td>47</td></tr><tr><td>3.5</td><td>Conclusion</td><td>49</td></tr></table>
## 3.1 Introduction

Motivated by the desire of reducing or avoiding accurate environmental modeling, robotic systems are more and more often equipped with external exteroceptive sensors, like cameras, that allow them to acquire geometric information on the environment in which they operate, without direct contact with it. This allows such systems to perform the required tasks without having an accurate preliminary knowledge of the environment.

The visual perception is a very versatile sensing modality and the extracted information can be used for many purposes ranging from motion planning, localization and mapping, scene segmentation, control, just to name a few. This kind of sensors becomes essential when a task has to be performed in a partially known/unknown environment or when it is not completely rigid. Cameras make then possible to account for environmental modeling errors as well as for inaccuracies in the identified geometric model of the manipulator.

A fundamental aspect that characterizes classical visual control, with respect to motion and force control, is the fact that the controlled quantities are not directly measured by the sensor, but are obtained from these downstream of rather complex image processing and computer vision algorithms. Such algorithms allow the extraction of digital information, known as visual features, relating to the objects present in the images of a scene, that can be used to estimate the position and orientation (pose) of the camera with respect to those objects.

Different other paradigms of visual control exist that do not make use of classical geometric features. For instance, [32] considers the luminance of the pixels of the entire image as visual feature; this approach is known as photometric visual servoing and belongs to the so called *direct visual servoing* methods. [33] proposed a generic framework to consider histograms as visual features while [34] presented a deep neural network-based method to perform 6 DOF visual servoing in which a Convolutional Neural Network was trained to estimate the relative pose between two images of the same scene.

The algorithmic and technological progress made over the past decades has led to tightly intertwine the aspects of perception with those of action, by directly integrating the measurements provided by a vision system into closed-loop control laws that deal with the extracted visual information. When visual measurements are used in feedback in a control algorithm, we speak of visual control or *Visual-Servoing*, and it is the main focus of this chapter. This term seems to be firstly introduced by Hill and Park [35] in 1979 to distinguish their approach from earlier "blocks world" experiments where the system alternated between picture
taking and moving, referred as *look-then-move*. That approach uses open-loop
visual measurements and therefore makes the system not very robust against un-
certainties due, for example, to the fact that the object may have moved while the
end-effector reaches the desired pose. Prior to the adoption of its current term,
Visual-servoing was generally referred to by the less specific term *visual feedback*.
The difference between visual-servoing and classical look-then-move systems can
be also seen from the data abstraction flow perspective from perception and ac-
tion [36] as shown in Figure 3.1.

<figure><img src="image_14.png" /><figcaption>**Figure 3.1** – General hierarchical control structure of a classical *look-then-move* approach (blue line) in contrast with a visual servoing system (dashed red line). Visual-servoing can be considered as a low-level “shortcut” through the hierarchy, characterized by high bandwidth but moderate image processing. Higher levels correspond to more abstract data representation and lower bandwidth.</figcaption></figure>

**3.2 Camera model**

Cameras are at the heart of any visual system and it is worth delving into their
mathematical characterization. A camera model describes the mathematical rela-
tionship between the coordinates of a point **X** = (X, Y, Z)ᵀ in three-dimensional
space and its projection on the image plane **x** = (x, y), see Figure 3.2. By far,
the most common camera model is the *pinhole model*, which makes the funda-
mental assumption that rays of light enter the camera through an infinitely small
aperture.

A scene's 3D point **X** is firstly transformed in camera frame Σ_c coordinates
(c**X** = [cX, cY, cZ]ᵀ) and then is projected, using a perspective transformation,
<figure><img src="image_15.png" /><figcaption>**Figure 3.2** – Frontal pinhole imaging model: the image of a 3-D point $^cX$ is the point $x$ at the intersection of the ray going through the optical center $O_c$ and the image plane at a distance $f$ in front of the optical center.</figcaption></figure>

into the image plane which forms the corresponding point $x = (x, y)$, we have:

$$ \begin{cases} x = {}^cX/{}^cZ = (u - c_x)/f\alpha \\ y = {}^cY/{}^cZ = (v - c_y)/f \end{cases} \qquad (3.1) $$

being $f$ the ratio between the focal length, which is the distance between the origin of the camera frame and the image plane, and the pixel vertical dimension, $\alpha$ the ratio of the pixel horizontal and vertical dimensions, $c_x$ and $c_y$ the coordinates of the principal point in pixels. $\bar{x} = (u, v)$ gives the coordinates of the image point expressed in pixel units, and $\mathbf{a} = (c_x, c_y, f, \alpha)$ is the set of camera intrinsic parameters. Further details on imaging geometry and perspective projection can be found in many computer vision text books [37, 38].

## 3.3 Visual Servoing

Visual servoing techniques consist of using the information provided by one or several cameras to control the movements of a robotic system. This allows to execute a wide range of tasks spanning from locating the system with respect to its environment, or tracking mobile objects, by controlling one or as many as all of the camera's $n \le 6$ degrees of freedom. Regardless the sensor's configuration, which can be both a mounted camera on the robot's end-effector (eye-in-hand) or one (several) scene camera(s) (eye-to-hand), the objective is twofold; on the
one hand, one must select a set of k possible visual data, in order to control the desired $m \le n$ degrees of freedom, while on the other hand, one must develop a control law capable of carrying these data $\mathbf{s}(t)$ to reach the desired value $\mathbf{s}^d$ which defines when the task has been properly executed. It is also possible to track a desired trajectory $\mathbf{s}^d(t)$. The aim of all vision-based control schemes is to minimize an error vector $\mathbf{e}_s = \mathbf{s}^d(t) - \mathbf{s}(t)$, that is, making this value reach zero and maintaining it there [39].

<figure><img src="image_16.png" /><figcaption>**Figure 3.3** – 2-D and 3-D visual servoing: 2-D visual servoing is used to bring the camera’s frame of reference from $\Sigma_{c^*}$ to $\Sigma_c$ based on measurements $\mathbf{s}$ extracted directly from the image (left). With 3-D visual servoing, the measurements $\mathbf{s}$ represent 3-D data estimated after a 3D localization phase (right)</figcaption></figure>

To study the system’s behavior, a model describing the relationship between the chosen visual data $\mathbf{s}(t) \in \mathbb{R}^k$ and the control variables is needed. The vector $\mathbf{s}^d$ contains the desired values of the features. Let us suppose the vector of visual data to be defined by a differentiable application $\mathbf{s} : SE(3) \to \mathbb{R}^k$:

$$ \mathbf{s} = \mathbf{s}(\mathbf{m}(t), \mathbf{a}) \qquad (3.2) $$

the vector $\mathbf{m}(t)$ represents a set of image measurements (e.g., the image coordinates of interest points or the image coordinates of the centroid of an object). These image data are utilized to generate a $\mathbf{s}(\mathbf{m}(t), \mathbf{a})$ vector of k visual features, where $\mathbf{a}$ is a set of parameters that reflect further information (e.g., coarse camera intrinsic parameters or 3-D models of objects). The visual datum can therefore be modified by the motion of either the camera or the object it perceives. Once $\mathbf{s}$ is chosen, the control scheme design can be quite simple designing, for example, a velocity controller. The relationship that links the relative twist $^c\mathbf{v} \in \mathbb{R}^6$ between the camera and the object with the rate of change of the visual data can be obtained differentiating equation (3.2), leading to:

$$ \dot{\mathbf{s}} = \frac{\partial \mathbf{s}}{\partial \mathbf{p}} \dot{\mathbf{p}} = \mathbf{L}_s \mathbf{c} \mathbf{v} = \mathbf{L}_s (\mathbf{c} \mathbf{v}_c - \mathbf{c} \mathbf{v}_o) = \mathbf{L}_s \mathbf{c} \mathbf{v}_c + \dot{\mathbf{s}}_o \qquad (3.3) $$
being $L_s \in \mathbb{R}^{k \times 6}$ the well-known interaction matrix [40] related to $s$, $^c v_c = [^c v_c^\top \ ^c \omega_c^\top]^\top$ and $^c v_o = [^c v_o^\top \ ^c \omega_o^\top]^\top$ the camera and object absolute twists in camera frame respectively, while $\dot{s}_o$ represents the contribution to the features' velocity due to the target motion.

From the previous equation (3.3) and considering the time derivative of the feature error for static target ($^c v_o = 0$) and constant desired features $\dot{s}^d = 0$, $\dot{e}_s(t) = \dot{s}^d - \dot{s}(t) = -\dot{s}(t)$, we immediately obtain

$$ \dot{e}_s(t) = -L_s \, ^c v_c \qquad (3.4) $$

Considering $^c v_c$ as the input to the manipulator controller ($^c v_c^{cmd}$), and to ensure an exponential decoupled decrease of the error, we ask as a control objective to have $\dot{e}_s = -\lambda e_s$, that together with equation (3.4) we obtain

$$ ^c v_c^{cmd} = \lambda L_s^\dagger e_s \qquad (3.5) $$

where $L_s^\dagger$ is chosen as the Moore-Penrose pseudo-inverse of $L_s$, that is, $L_s^\dagger = (L_s^\top L_s)^{-1} L_s^\top$ when $L_s$ is of full-rank (rank($L_s$) = 6). This choice allows $\|\dot{e}_s - \lambda L_s L_s^\dagger e_s\|$ and $\|^c v_c^{cmd}\|$ to be minimal. When $k = 6$, if $det(L_s) \neq 0$ it is possible to invert $L_s$, giving the control $^c v_c^{cmd} = \lambda L_s^{-1} e_s$. In real visual servo systems, it is impossible to know perfectly in practice either $L_s$ or $L_s^\dagger$. So an approximation or an estimation of one of these two matrices must be realized. In the sequel, we denote both the pseudoinverse of the approximation of the interaction matrix and the approximation of the pseudoinverse of the interaction matrix by the symbol $\tilde{L}_s^\dagger$. Using this notation, the control law is in fact:

$$ ^c v_c^{cmd} = \lambda \tilde{L}_s^\dagger e_s \qquad (3.6) $$

This is the basic design implemented by most visual servo controllers.

The main difference between visual servoing schemes is how features $s$ are designed. We will take a glance at two very different approaches in the next two subsections of this chapter. First, we describe image-based visual servo control (IBVS), which consists of a set of features that are readily available as image data. Then, we describe pose-based visual servo (PBVS) control, in which $s$ is a set of three-dimensional parameters that must be estimated from image measurements.

### 3.3.1 IBVS: Image-Based Visual Servoing

In Image-Based Visual Servoing (IBVS) the visual features consist of 2D primitives directly extracted from the image plane. Traditional control schemes use a set of points in image plane coordinates, but many other primitives can be adopted [40],
like straight lines or image moments [41]. The image measurements are usually obtained in pixel coordinates and the intrinsic parameters are used to go from pixels to the features $\mathbf{s}$.

For the case of image points features, it is possible to obtain the relative interaction matrix by time derivation of the perspective model equation (3.1) (see also Figure 3.2) and relating then the velocity of the 3D point to the camera spatial velocity using the well-known equation $^c\dot{\mathbf{X}} = -^c\mathbf{v}_c - ^c\omega_c \times ^c\mathbf{X}$, finally yielding $\dot{\mathbf{x}} = \mathbf{L}_x^c\mathbf{v}_c$, with

$$ \mathbf{L}_x(\mathbf{x}, ^cZ) = \begin{bmatrix} -1/{}^cZ & 0 & x/{}^cZ & xy & -(1+x^2) & y \\ 0 & -1/{}^cZ & y/{}^cZ & 1+y^2 & -xy & -x \end{bmatrix} \quad (3.7) $$

in which the value $^cZ$ is the depth of the point relative to the camera frame; therefore, any control scheme that uses this form of the interaction matrix must estimate or approximate this value. Similarly, the camera intrinsic parameters are involved in the computation of $\mathbf{x}$ and $\mathbf{y}$. Thus, $\mathbf{L}_x$ cannot be directly used and has to be estimated or approximated $\tilde{\mathbf{L}}_x$.

To control the 6 degrees of freedom we need at least six independent features to fully constrain the system. We can use then three image points ($k = 6$ two coordinates per image point), but given the singularity problems related to this choice, usually more than three points are used ($k \ge 6$). The feature vector is built as $\mathbf{s} = [\mathbf{x}_1^\top \mathbf{x}_2^\top \mathbf{x}_3^\top \dots]^\top$, while the interaction matrix is obtained by stacking the individual interaction matrices of each point of the type (3.7)

$$ \mathbf{L}_s(\mathbf{s}, \mathbf{Z}) = \begin{bmatrix} \mathbf{L}_x(\mathbf{x}_1, {}^cZ_1) \\ \mathbf{L}_x(\mathbf{x}_2, {}^cZ_2) \\ \mathbf{L}_x(\mathbf{x}_3, {}^cZ_3) \\ \vdots \end{bmatrix} \quad (3.8) $$

The same idea holds for any other primitive we want to use, like straight lines, which are represented in polar coordinates as shown in Figure 3.4

Results for more complex primitives (circles, spheres, and cylinders) are given in [42].

Once enough features have been chosen to constrain all the task's degrees of freedom, we can apply control equation (3.6).

### 3.3.2 PBVS: Pose-Based Visual Servoing

Pose-Based Visual Servoing (PBVS), as previously stated, uses the pose parameters of the frame attached to the camera with respect to another frame, usually
<figure><img src="image_17.png" /><figcaption>Figure 3.4 – 2D line representation in polar coordinates (ρ, θ).</figcaption></figure>

the object frame, to define the vector of visual features **s**. The camera intrinsic parameters and the 3-D model of the observed object must be known in order to compute its pose from a set of measurements in a single image. This is a classical problem in computer vision known as *3D localization problem*. The parameterization used to define **s** is thus determined by the representation adopted for the camera pose. We consider three different coordinate frames, the current camera frame Σ<sub>c</sub>, the desired camera frame Σ<sub>d</sub>, and a reference frame Σ<sub>o</sub> associated to the object. If <sup>c</sup>*T*<sub>o</sub> is the homogeneous transformation matrix that expresses the pose of the object in camera frame coordinates (Σ<sub>c</sub>), <sup>d</sup>*T*<sub>o</sub> the object’s pose in desired camera frame (Σ<sub>d</sub>), one can express the pose error between the current and the desired pose of the camera in the desired frame as

$$ ^{d}\mathbf{T}_{o} {}^{c}\mathbf{T}_{o}^{-1} = {}^{d}\mathbf{T}_{c} = \left[ \begin{array}{c|c} {}^{d}\mathbf{R}_{c}({}^{d}\boldsymbol{\phi}_{c}) & {}^{d}\mathbf{p}_{c} \\ \hline 0 & 0 \end{array} \right] \qquad (3.9) $$

where for the orientation representation <sup>d</sup>*φ*<sub>c</sub> = θ**u**, the axis/angle is adopted. The feature vector is then **s** = (<sup>d</sup>*p*<sub>c</sub>, θ**u**). In this case **s**<sup>d</sup> = 0 and **e**<sub>s</sub> = **s**, while the interaction matrix is

$$ \mathbf{L}_X = \begin{bmatrix} {}^{d}\mathbf{R}_c({}^{d}\boldsymbol{\phi}_c) & \mathbf{O} \\ \mathbf{O} & \mathbf{L}_{\theta u} \end{bmatrix} \qquad (3.10) $$

with

$$ \mathbf{L}_{\theta u} = \mathbb{I}_3 - \frac{\theta}{2}[\mathbf{u}]_{\times} + \left(1 - \frac{\mathrm{sinc}(\theta)}{\mathrm{sinc}^2(\frac{\theta}{2})}\right)[\mathbf{u}]_{\times}^2 \qquad (3.11) $$

where sinc(θ) = sin(θ)/θ.

This feature representation fully decouples the translational and rotational motions allowing to achieve straight line motions of the end-effector in Cartesian
space using the kinematic control law equation (3.6)

$$ ^{c}v_{c}^{cmd} = \lambda \widehat{L}_{X}^{-1} e_{s} \qquad (3.12) $$

### 3.3.3 Stability analysis

In order to assess the stability of the closed-loop visual servo systems, we can resort to the Lyapunov analysis by considering the candidate function defined by the squared error norm $V = \frac{1}{2}e_s^\top e_s$, whose derivative is given by

$$ \dot{V} = e_s^\top \dot{e}_s = -\lambda e_s^\top L_s \widehat{L}_s^\top e_s \qquad (3.13) $$

to which the error dynamics $\dot{e}_s = \dot{s} = L_s {}^{c}v_c$ and the controller ${}^{c}v_c^{cmd} = \lambda \widehat{L}_s^\top e_s$ have been replaced revealing that the global asymptotic stability of the system is obtained when the following sufficient condition is ensured:

$$ L_s \widehat{L}_s^\top > 0 \qquad (3.14) $$

If the camera degrees of freedom equals the number of features (i.e., $k = 6$), and if the features are chosen so that $L_s$ and $\widehat{L}_s^\top$ have full rank 6, then the previous condition is guaranteed if the approximation $\widehat{L}_s^\top$ is sufficiently accurate.

#### 3.3.3.1 Stability analysis in the IBVS case

Most of the IBVS cases consider a redundant number of features ($k > 6$) and therefore the stability condition stated before can never be attained. Since $L_s \widehat{L}_s^\top \in \mathbb{R}^{k \times k}$ can at most have rank($L_s \widehat{L}_s^\top$) = 6, it presents a non trivial null space. In this case, feature configurations $e_s \in \ker(\widehat{L}_s^\top)$ corresponds to local minima, thus only local asymptotic stability can be obtained and further analysis is required.

Let us define a new error $e'_s = \widehat{L}_s^\top e_s$ whose time derivative is

$$ \dot{e}'_s = \widehat{L}_s^\top \dot{e}_s + \frac{d\widehat{L}_s^\top}{dt} e_s \qquad (3.15) $$

and since, following [43], $\frac{d\widehat{L}_s^\top}{dt} e_s = \mathcal{O}(e_s)^c v_c$, it results

$$ \dot{e}'_s = (\widehat{L}_s^\top L_s + \mathcal{O}(e_s))^c v_c = -\lambda (\widehat{L}_s^\top L_s + \mathcal{O}(e_s)) e'_s \qquad (3.16) $$

where $\mathcal{O}(e_s) \to 0$ when $s \to s^d$, resulting in the condition

$$ \widehat{L}_s^\top L_s > 0 \qquad (3.17) $$
for ensuring local asymptotic stability. Indeed, only the linearized system $\dot{e}'_s = -\lambda \hat{L}_s^\dagger L_s e'_s$ has to be considered if we are interested in the local asymptotic stability.

Even though local asymptotic stability can be ensured when $k > 6$, we cannot ensure global asymptotic stability. Determining the size of the neighborhood where the stability and the convergence are ensured is still an open issue, even if this neighborhood is surprisingly large in practice [44].

On the other hand, when dealing with a large number of visual features ($k > 6$), their selection and configuration is an important aspect to not underestimate as they must be chosen carefully to avoid potential local minima in the workspace. It is indeed possible to get stuck into a given configuration when

$$ (s^d - s) \in \ker(\hat{L}_s^\dagger) \qquad (3.18) $$

since a zero velocity command will be generated even if $s \neq s^d$ ($e_s \neq 0$).

### 3.3.3.2 Stability analysis in the PBVS case

The stability properties of PBVS are quite appealing. Since $L_{\theta u}$ given in equation (3.11) is singular only for $\theta = 2k\pi$, stability condition equation (3.14) guarantees the global asymptotic stability under the strong hypothesis that all the pose parameters are perfectly known, leading to $L_X \hat{L}_X^{-1} = \mathbb{I}_6$. In practice, however, even small errors in computing the points position in the image can lead to pose errors that can impact significantly the accuracy and the stability of the system [44].

### 3.3.4 Mounted camera

If we consider a camera mounted on the end-effector of a manipulator observing a (possibly moving) object, the relation between the rate of change of the visual features $\dot{s}$, the joint velocities of the robot $\dot{q}$ and the target motion can easily be obtained as:

$$ \dot{s} = L_s c v = L_s c \mathbb{T}_e e J_e(q) \dot{q} - L_s c v_o = L_s c \mathbb{T}_e e J_e(q) \dot{q} - \dot{s}_o \qquad (3.19) $$

being $e J_e(q)$ the robot Jacobian in end-effector coordinates as seen in Section 2.2.1, and $c \mathbb{T}_e$ the twist-transformation matrix that transforms the end-effector twist from the end-effector frame to the camera frame (see Section 2.2.3). For the sake of readability, from now on the Jacobian dependency on the robot's configuration ($q$) will be omitted.
### 3.3.5 Feature trajectory tracking

Let now consider the case in which a desired feature trajectory $\dot{\mathbf{s}}^d(t)$ is planned. When path planning and trajectory following are combined, the visual servoing robustness with respect to modeling errors is considerably improved. The control laws seen in the previous subsections can be adapted to take into account the time-varying nature of the given trajectory such that the error $\mathbf{e}_s = \mathbf{s}^d - \mathbf{s}$ remain small. In particular, deriving the error we have now that

$$ \dot{\mathbf{e}}_s = \dot{\mathbf{s}}^d - \dot{\mathbf{s}} = \dot{\mathbf{s}}^d - \mathbf{L}_s^c \mathbf{v}_c \quad (3.20) $$

from which, by setting our control objective as before $\dot{\mathbf{e}}_s = -\lambda \mathbf{e}_s$, one obtains the following control law

$$ \mathbf{v}_c^{cmd} = \lambda \hat{\mathbf{L}}_s^\dagger \mathbf{e}_s + \hat{\mathbf{L}}_s^\dagger \dot{\mathbf{s}}^d \quad (3.21) $$

The control law's new second term anticipates the variation of $\dot{\mathbf{s}}^d$, eliminating the tracking error it would cause. When tracking a moving target, a similar form of the control law will be attained, as we will see in the next.

### 3.3.6 Target tracking

Let consider now the case of regulation to a constant desired value $\mathbf{s}^d$ of the features following a moving target. Deriving again the error vector we obtain this time

$$ \dot{\mathbf{e}}_s = \dot{\mathbf{s}}^d - \dot{\mathbf{s}} = -\mathbf{L}_s^c \mathbf{v}_c + \frac{\partial \mathbf{e}_s}{\partial t} \quad (3.22) $$

where the term $\partial \mathbf{e}_s / \partial t = \dot{\mathbf{s}}_o$ expresses the error variation due to the generally unknown motion of the target. Using the same control objective as usual, we obtain

$$ \mathbf{v}_c^{cmd} = \lambda \hat{\mathbf{L}}_s^\dagger \mathbf{e}_s + \hat{\mathbf{L}}_s^\dagger \frac{\partial \hat{\mathbf{e}}_s}{\partial t} \quad (3.23) $$

where $\hat{\mathbf{e}}_s$ is an estimation or approximation of the features velocity due to the target motion. We proposed an estimation model within an Extended Kalman Filter to estimate this term, which will be illustrated in Chapter 5. More details on this subject can be found in [45].

## 3.4 Second-order Visual Servoing

Visual Servoing has been widely investigated in the last decades as it provides a powerful strategy for robot control. Thanks to the direct feedback from spatial sensors, it allows to reduce the impact of some modeling errors. The main
drawbacks with visual servoing, and more generally sensor-based control, is that robots can only be controlled through its end-effector twist or its joints velocities. These schemes provide kinematic control laws that do not take the robot dynamics into account. These control laws ensure an exponential decrease of the monitored values in sensor-space, under the assumptions given in Section 3.3.3, but when they are not met, tracking performance and convergence behavior can be poor. Furthermore, despite the linear convergence in sensor space, the end-effector's motion is not predictable. Despite there exist varying gain strategies that modify the temporal evolution of the system, the operational space dynamic behaviour could not be satisfactory when the robot dynamics starts playing a role (e.g., for fast motions or when in contact with the environment).

Another approach that can be exploited consists in going one step further and to consider an acceleration model for the features. This strategy allows also to obtain a natural and direct link with the dynamic model of the controlled system, enabling full trajectory tracking in sensor-space.

In this section we briefly discuss the derivation of the second-order model for the visual-servoing that can be found in [46,47].

Equation (3.3) expresses the link between the rate of change of the features and the sensor's velocity and differentiating it, one obtains the expression relating the features acceleration and the sensor and object velocity and acceleration as:

$$ \ddot{s} = L_s (c \dot{v}_c - c \dot{v}_o) + \dot{L}_s (c v_c - c v_o) \quad (3.24) $$

or, when differentiating (3.19) (eye-in-hand mount), the link with the joint variables can be done:

$$ \ddot{s} = \dot{L}_s c \mathbb{T}_e^e J_e \dot{q} + L_s c \dot{\mathbb{T}}_e^e J_e \dot{q} + L_s c \mathbb{T}_e^e \dot{J}_e \dot{q} + L_s c \mathbb{T}_e^e J_e \ddot{q} - \ddot{s}_o. \quad (3.25) $$

Equation (3.25) can be rearranged in a more compact form as:

$$ \ddot{s} = J_s \ddot{q} + h_q - \ddot{s}_o \quad (3.26) $$

where $J_s = L_s c \mathbb{T}_e^e J_e$ denotes the so called Feature Jacobian [48] and:

$$ h_q = (\dot{L}_s c \mathbb{T}_e^e J_e + L_s c \dot{\mathbb{T}}_e^e J_e + L_s c \mathbb{T}_e^e \dot{J}_e) \dot{q}. \quad (3.27) $$

Notice that for a *eye-in-hand* mount configuration the relative pose between the end-effector and the camera is fixed, then the twist-transformation matrix is constant, leading to $c\dot{\mathbb{T}}_e = 0$.

Consider the manipulator model equation (2.27) in free space ($h_e = 0$)

$$ \tau = B \ddot{q} + N \quad (3.28) $$
in which we have compacted in $N = C\dot{q} + g + F_v\dot{q} + F_s\text{sgn}(\dot{q})$. The inertia matrix of the manipulator is a positive definite square matrix and is then always possible to invert. We can then explicit the joint acceleration from the previous $\ddot{q} = B^{-1}(\tau - N)$ equation and inject it in equation (3.26). For a motionless target $\ddot{s}_o = 0$, one obtains the link between the features acceleration and the joint actuation torques of the manipulator

$$J_s B^{-1} \tau = \ddot{s} - h_q + J_s B^{-1} N \quad (3.29)$$

## 3.5 Conclusion

In this chapter we have introduced the basics of visual control, also known as visual servoing, which represents the second cardinal point of this work. On the one hand, we illustrated the terminology and nomenclature used, while on the other, we highlighted the first and second order relationships that exist between the relative motion of the visual features and the camera. The concepts shown in this chapter are of paramount importance in regards of the development of the results carried out in this thesis, as we will see from Part II onwards.

The next chapter lays the groundwork for the vision/force coupling discussed in the literature, which we used as a starting point for our research.
CHAPTER

4

Vision-Force control

Contents

<table><tr><td>4.1 Introduction</td><td>52</td></tr><tr><td>4.2 Hybrid Vision-Force control</td><td>53</td></tr><tr><td>4.3 Visual-Impedance control</td><td>54</td></tr><tr><td>4.4 External Hybrid Vision-Force control</td><td>55</td></tr><tr><td>4.5 Constraint-based methods</td><td>58</td></tr><tr><td>4.6 Technological aspects</td><td>60</td></tr></table>
## 4.1 Introduction

The sensory integration of vision and force, which combines visual servoing with force control, is at the heart of this thesis work. Over the last two decades, much research has been done on combining visual servoing techniques with force control to take advantage of the complementarity of vision and force sensing. Cameras are capable of providing a rich description of the scene, while force sensing can provide local information about the contact itself. A combined vision/force arrangement is becoming increasingly common, as in the context of physical human-robot interaction [9], due to the greater computing power available and the ever lower cost of vision systems, greatly increasing the spectrum of robotic tasks that may be accomplished. However, due to the very different nature of these two sensing modalities (e.g., difference in the measured physical quantities, data rates, delays, etc.), obtaining an effective combined use of vision and force is not straightforward. Sensor integration approaches necessitate a shared representation of the sensory input being fused. This common data representation is not given for vision and force sensors [1]; the two sensing systems produce, as said before, fundamentally distinct quantities, namely force and position. Despite that, examples in which a combined use of vision and force sensing has been successfully achieved within an estimation (Bayesian [1, 49, 50] or incremental smoothing and mapping (iSAM) [51, 52]) framework are not isolated.

Transferring the problem from the perception level to the control level is another way to approach the problem of visual force sensor integration. Nelson [53] suggested three different ways for combining force and vision in a manipulator's feedback loop, which are: traded, hybrid, and shared control. For a given direction, traded control alternates between force control and visual servo in a mutually exclusive manner. Hybrid control, as for its position/force counterpart saw in Section 2.4.1, partitions the task directions into two orthogonal subspaces controlled separately by vision or force. Shared control offers the highest level of sensor integration allowing for simultaneous control with both vision and force along any task direction.

Most of the motion/force control strategies discussed in Chapter 2 have been revisited by researchers and expanded by essentially replacing the motion control loop with vision. As a result, those schemes may have the same strengths and limitations as their position-based counterparts, which we are about to discuss.
## 4.2 Hybrid Vision-Force control

Several hybrid vision/force control schemes have been proposed and applied [53]. All of them follow the same architecture shown in Figure 4.1, which is equivalent to the hybrid position/force control [14] presented in Section 2.4.1. The input to the vision control law (VCL) is computed from desired $\mathbf{s}^d$ and current $\mathbf{s}$ visual features. As for the position/force case, it is necessary to ensure orthogonality between vision $\mathbf{v}_v^d$ and force controlled subspaces $\mathbf{v}_f^d$ to avoid any conflict at the actuation level. Hybrid control separates vision control and force control into two separate control loops, that operate in orthogonal directions, as shown in Figure 4.1. A diagonal selection matrix $\mathbf{S}$ and its complementary ($\mathbb{I} - \mathbf{S}$) are introduced in the two control loops.

<figure><img src="image_18.png" /><figcaption>Figure 4.1 – Hybrid-based vision/force control scheme</figcaption></figure>

The selection matrices are applied to vision and force errors defined in a common frame in operational space. The resulting motion is given by the output contributions of vision and force as follows:

$$ \mathbf{v}_e^d = \mathbf{S}\mathbf{v}_v^d + (\mathbb{I} - \mathbf{S})\mathbf{v}_f^d \quad (4.1) $$

where $\mathbf{v}_v^d$ is the output of the Vision Control Law (VCL) according to the classical visual servoing kinematic controller seen in Chapter 3, and $\mathbf{v}_f^d$ is the output of the Force Control Law (FCL) taking into account the desired behavior of the system according to the relationship between force and differential displacement by means of the stiffness matrix ($\mathbf{h} = \mathbf{K} \mathbf{d}\mathbf{X}_e$; $\dot{\mathbf{X}}_e = \dot{\mathbf{s}}_{3D} = \mathbf{L}_X \mathbf{v}$, being $\mathbf{L}_X$ the interaction matrix corresponding to the 3D pose definition $\mathbf{X}_e$, see Section 3.3.2). Visual servoing is only possible with hybrid control in directions that are orthogonal to the force feedback directions. The positioning error generated by a force perturbation in the force controlled direction cannot be corrected by the vision-based
controller. The range of possible tasks for this controller, on the other hand, is limited to those that can be described in terms of constraint surfaces [15].

Several improvements to this scheme were made by Baeten and De Schutter [2] who have allowed shared control in a hybrid scheme using, whether possible, vision-based feedforward terms along the force controlled directions, and vice versa, to improve the overall performance of the task.

## 4.3 Visual-Impedance control

<figure><img src="image_19.png" /><figcaption>Figure 4.2 – Impedance-based vision/force control scheme</figcaption></figure>

As shown in Section 2.4.2, the impedance position/force control framework allows to a priori define the way the manipulator shall react with respect to external force disturbance. Using such a scheme, the six degrees of freedom can be simultaneously position- and force-controlled. A programmable mechanical impedance Z is thus provided to the end-effector:

$$F(p) = pZ(p)X(p) \qquad (4.2)$$

The robot is supposed to be equivalent to a mass-spring-damper second-order system whose transfer function is:

$$pZ(p) = Mp^2 + Dp + K \qquad (4.3)$$

where M, D and K are respectively the desired inertia, damping and stiffness matrices.

Impedance control can be accomplished in a variety of ways. Torque-based impedance control, in which the low-level loop is a torque loop, and position-based impedance control, in which a low-level position controller is used, are the two
main categories. Torque-based impedance control should be preferred for general purposes in terms of performance because it can provide a wide range of target impedances, including soft behaviors [54]. When this method was proposed [55], the authors stated that “torque-based impedance control was not relevant to most industrial applications, that used built-in joint position controllers in the robot’s hardware controller. Thus, a position based impedance controller has been selected”. By considering only a damping matrix in the position-based impedance control, the mechanical impedance becomes $Z(p) = D$, which is also known as accommodation control [56]. For vision/force impedance control (see Figure 4.2), the final control vector can be written as $v_e^d = v_v^d + v_f^d$, where $v_v^d$ is the kinematic screw computed by the vision control law, and $v_f^d$ is computed by the force control loop. Thus, the expression for impedance vision/force control can be written as:

$$v_e^d = -\lambda L_s^\dagger(s - s^d) + L_X^{-1}D^{-1}(e h_e^d - e h_e) \quad (4.4)$$

so that $\dot{X}_e = \dot{s}_{3D} = L_X v_f^d$.

With such a serial scheme, it is possible that $s \neq s^d$ and $e h_e \neq e h_e^d$ while $v_e^d = 0$. A local minimum can thus be attained or oscillatory phenomena can occur. Consider moving the end-effector parallel to the contact surface in order to reach the target. The vision system slips over the surface, guiding the end-effector to its final position without friction. The controller, however, would fail if friction were present. Indeed, the vision-based controller tangential velocity decreases as it gets closer to the final position and, because of the friction component, it will eventually become smaller than the opposite tangential velocity generated by the force feedback. To avoid the system from blocking with a pure damping impedance, the following condition must be met:

$$\|v_v^d\| > \|v_f^d\| = D \|e h_e\| \quad (4.5)$$

Friction produces static positioning error when using a proportional visual control law like the one in equation (4.4). Although a proportional integral controller could be used, combining integral correction with vision feedback and friction non linearities could result in instabilities or in the emergence of limit cycles.

## 4.4 External Hybrid Vision-Force control

The External Hybrid Vision-Force control scheme was proposed by Mezouar et al. [57] in 2007. This scheme is based on the concept of external control, proposed by Perdereau et al. [58] in 1993. In this control scheme, force and vision
controllers are superposed in a hierarchical way. The juxtaposition of the force control loop over the vision control loop provides various benefits over hybrid- and impedance-based vision/force control, therefore addressing their shortcomings. The manipulator is equipped with a wrist-mounted force/torque sensor and a mounted camera (eye-in-hand).

More specifically, the force control loop is closed around an internal vision control loop. The desired vector of image observation $\mathbf{s}^d$, used as input to the vision-based controller, is modified by the force controller output, which is a relative position $d\mathbf{X}_e$, projected on sensor space by means of the interaction matrix $\mathbf{L}_s$. When the end-effector moves in free space, the output of the force controller is null and the robot is controlled according to the vision-based controller output. Otherwise the force controller modifies the reference trajectory of visual observations by adding a relative displacement $ds$ to the reference. Let us see how this feature displacement $ds$ is obtained.

<figure><img src="image_20.png" /><figcaption>**Figure 4.3** – 1D contact on a surface. Contact is modeled as a spring of stiffness $k$. (a) The manipulator tip applies a force $f_A$ on a surface reaching position $x_A$. (b) The manipulator applies a greater force $f_B$ on the contact surface penetrating in the material $x_B$. The force variation $(f_B - f_A)$ produces a displacement variation $dx = x_B - x_A$ proportional to the stiffness $k$.</figcaption></figure>

Considering the 1D case in Figure 4.3, the force variation $(f_B - f_A)$ between the two configurations ((a) and (b)) is proportional to the environment stiffness $k$ and the displacement $dx$ according to the equation

$$f_B - f_A = k(x_B - x_A) = k \, dx \qquad (4.6)$$

then, the displacement variation can be obtained as $dx = \frac{1}{k}(f_B - f_A)$. For the 3D case, one has to consider the *pose* variation (error $d\mathbf{X}_e$) of the manipulator’s end-effector in contact with the environment, while forces translate to *wrenches*. In order to properly project afterwards the 3D displacement in the feature space, one need to express all the involved *twists* and *wrenches* in the same reference
frame. Since the camera is rigidly attached to the last link of the manipulator, the relative pose between the camera and the end-effector frames is constant, allowing to express the wrench acting on the end-effector into the camera frame thanks to the wrench transformation matrix ($^c\mathbf{h}_c = {}^c\mathbb{F}_e {}^e\mathbf{h}_e$) [13]. So, equation (4.6) in 3D becomes

$$ \mathbf{h}_B - \mathbf{h}_A = \mathbf{K} d\mathbf{X}_e \quad (4.7) $$

where $d\mathbf{X}_e$ is the end-effector pose error vector associated to the Homogeneous transformation matrix $^A\mathbf{T}_B(\mathbf{X}_A, \mathbf{X}_B) = {}^w\mathbf{T}_A^{-1}(\mathbf{X}_A)^w\mathbf{T}_B(\mathbf{X}_B)$, and depends on the chosen orientation representation (in our case, the Axis/angle representation have been used). Since we are expressing all the quantities in the camera frame, in analogy with the pose-based visual servoing with 3D pose errors as features, the differential relationship between the error variation and the twist of the camera is

$$ \dot{\mathbf{s}}_{3D} = \mathbf{L}_{\mathbf{X}} {}^c\mathbf{v}_c \rightarrow \frac{d\mathbf{X}_e}{dt} = \mathbf{L}_{\mathbf{X}} \frac{d\mathbf{X}}{dt} \rightarrow d\mathbf{X}_e = \mathbf{L}_{\mathbf{X}} d\mathbf{X} \quad (4.8) $$

which replaced in equation (4.7) and isolating the infinitesimal twist $d\mathbf{X}$ yields to

$$ d\mathbf{X} = \mathbf{L}_{\mathbf{X}}^{-1} \mathbf{K}^{-1}(\mathbf{h}_{B} - \mathbf{h}_{A}) \quad (4.9) $$

Equation (4.8) holds true no matter which orientation representation is used, but the Jacobian that projects the velocities (in our case $\mathbf{L}_{\mathbf{X}}$) will change accordingly. One obtains then an infinitesimal displacement of the camera frame due to the forces/torques acting on this frame which have to be now projected in the feature space. By using the infinitesimal relationship in equation (4.8) but for a generic image feature ($\dot{\mathbf{s}} = \mathbf{L}_s {}^c\mathbf{v}_c$) and replacing equation (4.9), one finally obtain the projected displacement in image space as:

$$ ds = \mathbf{L}_s \mathbf{L}_{\mathbf{X}}^{-1} \mathbf{K}^{-1}(\mathbf{h}_B - \mathbf{h}_A) \quad (4.10) $$

which is the Force Control Law (FCL) used in the control scheme in Figure 4.4, where $\mathbf{h}_A$ and $\mathbf{h}_B$ are respectively the measured $^c\mathbf{h}_c = {}^c\mathbb{F}_e {}^e\mathbf{h}_e$ and the desired end-effector wrenchs $^c\mathbf{h}_c^d = {}^c\mathbb{F}_e {}^e\mathbf{h}_e^d$ projected in camera frame. On the other hand, the Vision Control Law (VCL) is the classical visual-servoing controller, saw in Chapter 3, that minimizes the error between the vector of the current features $\mathbf{s}$ and the vector of visual reference which was modified $\mathbf{s}^* = \mathbf{s}^d + ds$. It finally outputs the desired end-effector twist $\mathbf{v}_e^d$.

In [57], authors have provided an exhaustive comparison of the External hybrid scheme against Hybrid- and Impedance-based vision/force schemes showing how this controller converges without problems whether the others have failed. The hierarchical juxtaposition of the force control loop on the vision control loop provides
<figure><img src="image_21.png" /><figcaption>Figure 4.4 – External hybrid vision/force control scheme</figcaption></figure>

several advantages according to the existing methods, i.e., selection matrices and time-dependent geometric transformations are eliminated from the control loop, leading to a controller design independent of the arm configuration. Since the force control only acts on the reference trajectory, conflicts between force and vision controller are avoided. Furthermore, it has no effect on closed loop behavior, and therefore in its stability properties, since the wrench input can be viewed as a reference trajectory modifier [59].

As far as the external force loop is concerned, it should be stressed that it has no effect on the reference trajectory as long as the manipulator is moving in free space. The controller’s behaviour and stability properties are exactly those of a classical visual servo scheme (inner loop), as shown in Chapter 3. Visual servoing schemes rely on the low-level manipulator’s velocity controller to servo the robot. Such low-level controllers are usually high stiffness PID-like controllers implemented such as to guarantee that $q(t) \approx q_d(t)$ and the manipulator can be considered as an ideal positioning device. Once the end-effector comes into contact with the environment, the sensed wrench is transformed into a feature displacement $ds$ through the inverse of the contact stiffness $K$ (see equation (4.10)). If a soft contact behaviour is required for the manipulations task, the combination of the high stiffness of the joint controller and the consequent high gains involved in the external loop ($K^{-1}$) could lead to instability as we will show in Section 6.6.3.

## 4.5 Constraint-based methods

Constraint-based methods are an alternative way to integrate vision and force sensing. There are different approaches in this category to perform the sensory integration, like the Stack-of-Task [60] or the Whole-body control framework [61].
The Stack-of-Task framework [60] is a generalized inverse kinematics abstraction layer that creates a hierarchical organization of different tasks, allowing them to be executed, for example, giving higher priority to critical tasks. The Whole-body control framework [61] formulation allows for secondary task objectives to be controlled without dynamically interfering with the operational task.

Despite the fact that these frameworks were not really designed for sensory integration, but rather for synthesizing the control commands that must comply with different types of constraints imposed on the system, they have been successfully used by [62] for combining vision and haptic (force) cues by coupling a visual servoing controller with an impedance controller within the Stack-of-Task framework [60]. In [63] it was shown that visual servoing tasks can be integrated into a prioritized constraint-based torque control framework formulating it as just another task/constraint for the whole-body controller [61] that can be combined with force control tasks.

Another family of more general constraint-based methods meant for specifying complex tasks of general sensor-based robot systems are represented by [64, 65]. The iTaSC framework [64], emerged as a specification formalism to generalise and extend existing frameworks like the Operational Space Approach [11], Task Function Approach [66], Task Frame Formalism [67], geometric Cartesian Space control, and Joint Space control. In this framework, the use of feature coordinates, defined with respect to object and feature frames, facilitates the task specification and the introduction of uncertainty coordinates to model geometric uncertainty. The etasl/etc [65] framework, is based on feature variables and the concept of expression graphs. It avoids some of the common pitfalls in previous frameworks, and provides a flexible and composable way to define robot control tasks. Those frameworks allow over-specifying constraints along a given direction and resolve the priority conflicts by adding weights to such constraints. The underlying optimization solver provides the joint commands. Using the [64] framework formalism, [68] reformulated the image-based visual servoing as a constraint-based robot task, allowing for potential integration with force controlled tasks using such framework.

We do not take into account any of these constraint-based strategies because the focus of our research regards the pure sensory coupling aspects. Furthermore, these frameworks have a high computational burden, limiting their applicability to a robot like the one used in this study, which requires a control rate of 1kHz to operate, if no further measures are taken.
## 4.6 Technological aspects

In the last 20 years the technological landscape of this field has changed considerably. While Baeten [2] stated at the beginning of the century that vision and force systems were rather confined to research areas with a few exceptions in the industrial field, today, we can see that some manufacturers already provide their own solutions deploying these systems directly integrated into their robots, as we can see in Figure 4.5. Other companies on the other side, provide solutions with standardised interfaces that can be adapted to different robots, as the *Robotiq*<fnref n="1" /> solution for *Universal Robots*<fnref n="2" /> shown in Figure 4.6, thus underlining the interest in adopting this technology in industry due to their potential.

<figure><img src="image_22.png" /><figcaption>**Figure 4.5** – (a) Doosan Cobot M-Series: it is equipped with an optional “in house” smart vision module. (b) Cobot Omron TM: is equipped with a screwing tool and a vision system with lighting function for image enhancement. (c) ABB YuMi presents an integrated camera on its gripper. All this robots are equipped with joint torque sensors.</figcaption></figure>

<footnote n="1">¹https://robotiq.com/</footnote>

<footnote n="2">²https://www.universal-robots.com/</footnote>
<figure><img src="image_23.png" /><figcaption>**Figure 4.6** – Robotiq integrated solution for cascading wrist-mounted Force/Torque sensor, vision system and two finger gripper designed for the Universal Robots.</figcaption></figure>

The adoption of built-in torque sensors into the joints and the significant advances in collision detection techniques [69] have allowed manipulators to break out of the safety cages in which they were confined to work, allowing them to be used alongside human operators with a high degree of safety. Joint torque sensors also allows to estimate the external wrench at the end-effector, avoiding the need of an external Force/Torque sensor, although they are less accurate. Despite the increasingly massive use of cameras in robotics applications, the use of combined techniques of vision and force sensing does not present today the same level of maturity as that demonstrated by both the collision detection and the use of force sensing for interaction tasks. Cameras appear to be used primarily for objects detection in the workspace and afterwards the interaction is managed using traditional force control approaches (e.g., impedance control) not fully exploiting the capabilities and complementarity of both sensors during all the execution phases of the task at hand.

The objective of this thesis work is to provide new integrated vision/force control strategies to meet market demands for this type of integrated solution by closing the gap between existing technology and available control techniques.

In the next part, we deal with the problem of *shared* sensor integration, proposing both dynamic (Chapter 5) and kinematic (Chapter 6) controllers that “fuse” the sensing modalities in sensor space while we tackle several aspects related to the implementation of the dynamic controller.
# Part II Feature Space Compliance
Compliant motion occurs whenever a manipulator performs a task while main-
taining the contact between the end-effector and the environment. When imposing
a PD-like controller to the manipulated system, its behaviour is similar to a *mass-spring-damper* under the action of an external force. This part of the thesis is
devoted to illustrate our contributions on the derivation of controllers that can
deal with compliant motion along/about the visual features defining the visual
task, see Figure Part II. In the case of PBVS, a 3D pose reconstruction phase is
performed to estimate the position and orientation of the observed object in *operational space*. The visual feature in this case is the pose error vector between the
actual and desired camera frames. In the IBVS case instead, the visual features
(points, lines, moments, etc.) are directly extracted on the image plane. In this
work we refer to the more general space of all possible kinds of visual features as
the *Visual Feature Space*.

<figure><img src="image_24.png" /><figcaption>**Figure Part II** – Compliance achieved in different spaces: a PD-like controller brings the actual configuration (green dot) into the desired configuration (red dot), from left to right, in the joint space, in the operational space, in the operational space in the case PBVS and in the image space. We refer to the *Feature Space* as the more general space containing any kind of visual features, being them the parameters of a 3D pose, of image points, image lines, image moments, etc.</figcaption></figure>
CHAPTER

5

Feature Space Impedance

Contents

<table><tr><td>5.1</td><td>Introduction</td><td>66</td></tr><tr><td>5.2</td><td>Manipulator model in feature space</td><td>67</td></tr><tr><td>5.3</td><td>Impedance control in feature space for a static target</td><td>68</td></tr><tr><td>5.4</td><td>Impedance control in feature space for a moving target</td><td>72</td></tr><tr><td>5.5</td><td>Increasing the visual data rate and estimating the target's motion</td><td>73</td></tr><tr><td>5.5.1</td><td>EKF for IBVS with image points features</td><td>74</td></tr><tr><td>5.5.2</td><td>EKF for PBVS</td><td>75</td></tr><tr><td>5.6</td><td>Simulations</td><td>76</td></tr><tr><td>5.6.1</td><td>Task description and controllers implementation</td><td>77</td></tr><tr><td>5.6.2</td><td>Simulation Results</td><td>78</td></tr><tr><td>5.7</td><td>Real experiments</td><td>82</td></tr><tr><td>5.7.1</td><td>Implementation issues</td><td>82</td></tr><tr><td>5.7.2</td><td>Results</td><td>85</td></tr><tr><td>5.8</td><td>Summary</td><td>87</td></tr></table>
## 5.1 Introduction

As discussed in Chapter 3, Visual Servoing is a consolidated control technique that allows to precisely position a vision sensor with respect to an observed object in the scene. In the past decades, most research efforts focused on the modeling of the existing relationships between descriptive features in the image and the motion of the sensor at kinematic level [40,41], and in the development of control strategies to guide the chosen visual features towards the desired ones as summarized in [44,45]. Although much research has been conducted in this sense, little attention has been devoted to the use of second order models linking the features accelerations to the robot torques via the robot dynamical model, with some early attempts dating back more than two decades [40,70] up to some more recent contributions [46,47]. As well-known, explicitly taking into account the robot dynamics allows to design controllers with superior performance, especially for what concerns the regulation of forces and interaction with the environment. For instance, in many industrial applications the interaction tasks are executed in static or quasi-static conditions and can be solved at kinematic level. However if one wishes to reduce the execution time, faster motions are required making the control task essentially dynamic [71].

Most of the above cited works mainly focused on the motion of the robot in free space without taking into account possible physical interactions with the environment. Indeed while [70] only compensates for the air drag forces acting on the blimp, in [46], second-order visual-servoing is used to control solely the unconstrained directions of an engraving task within a hybrid control scheme [14] with a pure force controller acting along/about the constrained directions.

The goal of this chapter is to study the possibility of using second-order visual servoing for interaction control, since, as we will show in Section 5.3, a second-order visual-servoing controller under interaction with the environment boils down to an impedance controller in the visual feature space, which is a force control scheme belonging to the category of indirect force control methods [13].

In [5] which is further developed in the next chapter, we proposed an alternative to the impedance control in feature space - which results to be complex to implement given the limitations discussed and addressed in this chapter - by defining an admittance in feature space.

A contribution of this work is, therefore, to overcome these difficulties (i.e., low camera data rate, feature extraction delays, ill-conditioning of the task Jacobian, etc) and propose a true second-order visual servoing in feature space that can also take into account the possible, and generally unknown, motion of the observed target object to interact with the environment. This is achieved by modeling the
motion of the target in a second-order visual-servoing setting and by estimating its unknown motion via a suitable Extended Kalman Filter (EKF). The EKF is also instrumental for virtually increasing the data rate of the employed camera by providing high-rate estimations of the quantities needed by the derived torque-level controller, without resorting to dedicated high frequency, and therefore expensive vision systems like those in [4, 72]. The derivations of the filters' states are specialized for the cases of Image-Based Visual Servoing (IBVS) with image points as visual features and Pose-Based Visual Servoing (PBVS) while their respective similarities and differences are pointed out.

The filters derived in this chapter can also be easily integrated into the control scheme presented in the next chapter, both in case of static or moving target, although the discussion and respective experiments were carried out considering static targets only. The use of such filters, and the consequent increased data rate, would make the system even more responsive, particularly on rigid contacts.

## 5.2 Manipulator model in feature space

In this section, the manipulator's model in feature space under interaction with the environment is derived thanks to a suitable coordinates transformation exploiting the second order visual-servoing model derived in Section 3.4

The *Lagrangian* equation of motion of the manipulator (2.27) is a set of n nonlinear and coupled second-order differential equations for which the *inverse dynamics control* is a well-known control strategy for trajectory tracking aiming at linearizing and decoupling the manipulator's dynamics via feedback linearization, assuming that its model is perfectly known. We can rewrite equation (2.27) such as to explicit the joint accelerations:

$$ \ddot{q} = B(q)^{-1}(\tau - N(q, \dot{q}) - e^{J_e^\top e} h_e) \quad (5.1) $$

having compacted in $N(q, \dot{q}) = (C(q, \dot{q})\dot{q} + g(q))$ the terms relatives to the Coriolis, centrifugal and gravitational force effects. After replacing equation (5.1) into the second order visual servoing equation (3.26) and considering the observed target to be motionless ($e^{c}v_o = 0$), we find the law that governs the features motion:

$$ J_s B(q)^{-1} \tau = \ddot{s} - h_q + J_s B(q)^{-1} N(q, \dot{q}) + J_s B(q)^{-1} e^{J_e^\top e} h_e \quad (5.2) $$

Taking into account the expression of the *feature Jacobian* $J_s = L_s e^{c}T_e e^{J_e}$, the wrench-transformation matrix that projects the external wrench acting on the camera into the end-effector $e^{c}h_e = e^{c}F_c e^{c}h_c$, and recalling the relationship between
the *twist* and *wrench* transformation matrices $^c\mathbb{T}_e^\top = {}^e\mathbb{F}_c$ [13], we can rearrange the last member of (5.2) as:

$$L_s {}^c\mathbb{T}_e {}^e\mathcal{J}_e \mathcal{B}(q)^{-1} {}^e\mathcal{J}_e^\top {}^c\mathbb{T}_e^\top {}^c\mathcal{H}_c = L_s \mathcal{B}_c(q)^{-1} {}^c\mathcal{H}_c \quad (5.3)$$

where $\mathcal{B}_c(q)^{-1} = {}^c\mathbb{T}_e {}^e\mathcal{J}_e \mathcal{B}(q)^{-1} {}^e\mathcal{J}_e^\top {}^c\mathbb{T}_e^\top$ is the inverse of the manipulator inertia matrix projected in the camera frame. Equation (5.3) highlights how the external wrench acting on the camera frame and expressed in the same frame $^c\mathcal{H}_c$, projects in feature space as a virtual per unit of mass/inertia forces/torques (p.u.m.i.) (accelerations) acting on the image features. By renaming some terms in (5.2) as:

$$f_s = \mathcal{J}_s \mathcal{B}(q)^{-1} \tau \quad (5.4a)$$

$$b_s = \mathcal{J}_s \mathcal{B}(q)^{-1} \mathcal{N}(q, \dot{q}) \quad (5.4b)$$

$$f_{sext} = L_s \mathcal{B}_c(q)^{-1} \mathcal{H}_c \quad (5.4c)$$

one obtains the manipulator model in feature space:

$$\ddot{s} - h_q + b_s + f_{sext} = f_s \quad (5.5)$$

The obtained model presents an analogy with the joint space model (2.27) but the acceleration term $\ddot{s}$ do not present any inertia matrix, this means that the model is inertia-normalized, and the quantities in equation (5.5) are not forces and/or torques but they are relative forces and/or torques per unit of mass/inertia; indeed they have the physical dimension of an acceleration (m/s²).

## 5.3 Impedance control in feature space for a static target

As we have seen in Chapter 2, impedance control aims at achieving a dynamic behaviour of the robot end-effector in contact with the environment equivalent to a mass-spring-damper system subject to an external force. Our purpose here is to replicate this behaviour between the current and desired visual features lying on the image plane. Defining the image feature error vector $e_s = (s^d - s) \in \mathbb{R}^k$ as the difference between the desired $s^d$ and current $s$ visual features, or, in the case of 3D pose, $e_s \in \mathbb{R}^6$ as the pose error vector between the current $\Sigma_c$ and desired $\Sigma_d$ camera frames as depicted in Figure 5.1, we formally want to obtain the following behaviour in feature space:

$$M_s \ddot{e}_s + D_s \dot{e}_s + K_s e_s = f \quad (5.6)$$
<figure><img src="image_25.png" /></figure>

<figure><img src="image_26.png" /><figcaption>**Figure 5.1** – Desired mass-spring-damper behavior in the case of 3D pose (left) and four image point visual features (right). The current $\Sigma_c$ and desired $\Sigma_d$ camera frames are sketched as well as the object frame $\Sigma_o$. The current $\mathbf{s}$ (green) and the desired $\mathbf{s}^d$ (red) image point features in the image plane are depicted. The target is arranged in a coplanar square shape.</figcaption></figure>

where $M_s, D_s$ and $K_s$ are positive definite $k \times k$ matrices representing the relative per unit mass/inertia virtual mass, viscous and stiffness parameters of the impedance equation while $\mathbf{f} \in \mathbb{R}^k$ is a vector of per unit mass/inertia virtual forces (accelerations) acting on the features.

Let us suppose at this stage not to have any force measurement. The new control input for the manipulator model in feature space (5.5) is the vector of per unit of mass/inertia virtual forces $\mathbf{u}_s = \mathbf{f}_s$ which is chosen to compensate for any dynamic term in the model (5.5). One then has:

$$ \mathbf{u}_s = \alpha - \mathbf{h}_q + \mathbf{b}_s \qquad (5.7) $$

and by injecting it back into (5.5) yields:

$$ \alpha = \ddot{\mathbf{s}} + \mathbf{f}_{sext} \qquad (5.8) $$

in which $\alpha \in \mathbb{R}^k$ represents a resolved acceleration in feature space and constitutes the new control input that has to be opportunely designed. A natural choice for $\alpha$ is a PD controller with acceleration *feed-forward*.

$$ \alpha = \ddot{\mathbf{s}}^d + \mathbf{D}_s \dot{\mathbf{e}}_s + \mathbf{K}_s \mathbf{e}_s \qquad (5.9) $$

that replaced in (5.8) leads to the *closed-loop dynamics* of the system:

$$ \ddot{\mathbf{e}}_s + \mathbf{D}_s \dot{\mathbf{e}}_s + \mathbf{K}_s \mathbf{e}_s = \mathbf{f}_{sext} \qquad (5.10) $$

The behavior of the closed-loop system is the one sought in (5.6) in which $M_s$ coincides with the identity matrix. In static conditions $\ddot{\mathbf{e}}_s = \dot{\mathbf{e}}_s = 0$, when the manipulator is in free space ($\mathbf{f}_{sext} = 0$), it is easy to verify that the current position of the visual features reaches the desired one ($\mathbf{s} = \mathbf{s}^d$) while when it is
in contact with the environment, the static error is inversely proportional to the relative per unit of mass/inertia stiffness matrix $K_s$. Indeed, when $f_{sext} \neq 0$ in equation (5.10), the position error of the visual features is given by:

$$e_s = K_s^{-1} f_{sext} = K_s^{-1} L_s B_c(q)^{-1} h_c \quad (5.11)$$

As we can see from the previous equation, the error is configuration dependent due to the presence of the inertia matrix of the robot projected in camera frame.

By replacing (5.9) into (5.7) in which the feature space per unit of mass/inertia force/torque vector $u_s$ has the form (5.4a), leads to the resulting joint space controller:

$$u_{\tau} = (J_s B(q)^{-1})^{\dagger}(\ddot{s}^d + D_s \dot{e}_s + K_s e_s + J_s B(q)^{-1} N(q, \dot{q}) - h_q) \quad (5.12)$$

Even though we have considered the manipulator interacting with the environment in our derivation, if we do not have any force measurement, we arrive at the same control law derived in [46, 47]. In fact, this is analogous to the impedance control in the operational space case which is an inverse dynamic controller for trajectory tracking in free space whose properties when in contact with the environment have been taken into account.

Note that, in analogy to what was said in Subsection 3.3.3, in the IBVS case with $k > 6$ features, there is the theoretical risk, although very unlikely in practice, to specify unfeasible behaviours when the commanded feature space acceleration lies in $ker((J_s B(q)^{-1})^{\dagger})$ (see (5.12)). If for instance, one starts from a static position and servo the robot to reach a desired constant one with respect to a motionless target, from (5.12) one has $u_{\tau} = (J_s B(q)^{-1})^{\dagger}(K_s e_s)$; if $K_s e_s \in ker((J_s B(q)^{-1})^{\dagger})$, the generated torque command $u_{\tau} = 0$ even if $e_s \neq 0$.

The drawback of this controller (5.12) is that the closed-loop system (5.10) exhibits a configuration-dependent compliant behavior due to the presence of the inertia matrix of the manipulator in the external wrench projection into the feature space (see (5.11)). An easy way to overcome this issue and render the manipulator behaviour isotropic with respect to the external wrench, i.e., letting a given external wrench in Cartesian space be mapped into the image space generating the same per unit of mass/inertia force/torque vector regardless of the manipulator's configuration, is to measure the external forces being applied on the robot and fully compensate for them. This will make the manipulator infinitely rigid with respect to the measured forces but, thanks both to the fact that we have this measure and that we know how to project the wrenches into the feature space, we can impose the desired system's behavior during the interaction.
Let us suppose to have a wrist mounted force/torque sensor that provides the measured external wrench at the manipulator’s end-effector $^{e}h_e$. We can now add it in the feature space controller (5.7) to fully compensate for those forces as:

$$ \mathbf{u}_s = \alpha - \mathbf{h}_q + \mathbf{b}_s + \underbrace{\mathbf{L}_s \mathbf{B}_c(\mathbf{q})^{-1} \overbrace{\mathbf{c} \mathbb{F}_e^e \mathbf{h}_e}^{\mathbf{c} \mathbf{h}_c}}_{\mathbf{f}_{s_{ext}}} \quad (5.13) $$

Plugging this back again into (5.5), one obtains:

$$ \ddot{\mathbf{s}} = \alpha \quad (5.14) $$

For control purposes it is convenient to impose a constant apparent inertia matrix of the camera in order to have an homogeneous behavior along the Cartesian directions of the camera frame and manage the compliance along the visual features by opportunistly tuning the relative stiffness $K_s$ and damping $D_s$ matrices of the impedance controller.

<figure><img src="image_27.png" /><figcaption>**Figure 5.2** – By assigning a constant apparent inertia to the end-effector or camera frame, we impose the desired interaction behavior along/about the Cartesian directions. We choose an homogeneous inertial behavior so that the end-effector is equivalent to a sphere of mass. Applied forces on the end-effector (purple arrow on the left image) will be homogeneously distributed among the visual features (small arrows acting on the image lines (top-right) and image points (bottom-right)).</figcaption></figure>

This can be done by choosing as resolved accelerations in feature space the controller:

$$ \alpha = \ddot{\mathbf{s}}^* + \mathbf{D}_s \dot{\mathbf{e}}_s + \mathbf{K}_s \mathbf{e}_s - \mathbf{L}_s \bar{\mathbf{B}}_c^{-1} \mathbf{c} \mathbb{F}_e^e \mathbf{h}_e \quad (5.15) $$

being $\bar{\mathbf{B}}_c = \text{diag}(m_{c_d}, m_{c_d}, m_{c_d}, J_{c_d}, J_{c_d}, J_{c_d})$ in which $m_{c_d}$ [kg] and $J_{c_d}$ [kg.m²] are respectively the desired apparent mass and inertia that the camera should exhibit. Industrial robots typically interact with the environment using a tool (the tool tip), it can be useful then to exhibit isotropy on the end-effector frame (tool tip frame) by choosing a constant diagonal inertia matrix $\bar{\mathbf{B}}_e$, as done before for $\bar{\mathbf{B}}_c$,
and then project it in the camera frame as $\bar{B}_c^{-1} = {}^cT_e \bar{B}_c^{-1} {}^cT_e^\top$. The resulting joint space controller is therefore:

$$ u_{\tau} = (J_s B(q)^{-1})^{\dagger} \left( \ddot{s}^* + D_s \dot{e}_s + K_s e_s + J_s B(q)^{-1} N(q, \dot{q}) - h_q + f_{s_{ext}} - L_s \bar{B}_c^{-1} {}_cF_e^e h_e \right) \quad (5.16) $$

The closed-loop dynamics of the system is obtained by replacing (5.15) into (5.14) resulting in:

$$ \ddot{e}_s + D_s \dot{e}_s + K_s e_s = L_s \bar{B}_c^{-1} {}_cF_e^e h_e \quad (5.17) $$

Now, the projected external forces in feature space do not depend on the manipulator's configuration anymore (see (5.11)). This translates in a more intuitive tuning of the impedance parameters since this controller would make the manipulator under interaction with the environment behave in the same manner no matters its configuration while preserving the same stability properties of the previous controller (5.12).

## 5.4 Impedance control in feature space for a moving target

In this section we will consider the more general case of tracking a moving target and, for simplicity of treatment, we will suppose not to have any force/torque sensor mounted on the robot but that at least an estimate of the features' velocity and acceleration due to the target motion ($\dot{s}_o, \ddot{s}_o$) is available. In the next section we will detail how this estimation is obtained. Notice that the case in which a Force/torque sensor is available is formally analogous.

As done in the previous section, replacing (5.1) into the full equation of the second-order visual-servoing (3.26) results into:

$$ J_s B(q)^{-1} \tau = \ddot{s} + \ddot{s}_o - h_q + J_s B(q)^{-1} N(q, \dot{q}) + J_s B(q)^{-1} {}_e J_e^\top h_e \quad (5.18) $$

for which we rename the terms as in (5.4a)-(5.4c) obtaining:

$$ \ddot{s} + \ddot{s}_o - h_q + b_s + f_{s_{ext}} = f_s. \quad (5.19) $$

We choose then a feature space control input $u_s$ such as to compensate for any dynamic term, including the (usually unknown) acceleration term due to the target's motion

$$ u_s = \alpha - h_q + b_s + \ddot{s}_o \quad (5.20) $$
We then use the same acceleration resolved controller $\alpha$ as in equation (5.9) and recalling the feature error definition $e_s = s^d - s$, its time derivative $\dot{e}_s = \dot{s}^d - \dot{s}$ and equation (3.19), leading to the joint space torque controller

$$ u_{\tau} = (\mathbf{J}_s \mathbf{B}(\mathbf{q})^{-1})^{\dagger} \left( \ddot{\mathbf{s}}^* + \ddot{\mathbf{s}}_o + \mathbf{D}_s (\dot{\mathbf{s}}^d - \mathbf{J}_s \dot{\mathbf{q}} + \dot{\mathbf{s}}_o) + \mathbf{K}_s (\mathbf{s}^d - \mathbf{s}) + \mathbf{J}_s \mathbf{B}(\mathbf{q})^{-1} \mathbf{N}(\mathbf{q}, \dot{\mathbf{q}}) - \mathbf{h}_q \right) \quad (5.21) $$

where the new terms $\ddot{s}_o$ and $\mathbf{D}_s \dot{s}_o$ compensate for the target motion.

All the considerations we have done in the previous section concerning the dependency on the manipulator's configuration on the wrench mapping into the feature space holds true. Following the same reasoning done before we can obtain an isotropic and configuration independent interaction behaviour, also in the case of a moving target, by measuring the contact forces/torques.

## 5.5 Increasing the visual data rate and estimating the target's motion

In the previous section, we derived a controller that linearizes and decouples the dynamics of the system via feedback linearization but, even though most of the terms in the controller can be easily computed, those related to the target own motion are usually unknown and must thus be estimated. Since the target motion is generally unknown, the simplest assumption one can do is to consider a constant acceleration model corrupted by Gaussian noise. The target motion can be treated as a disturbance [73] of the nominal motion of the features due to the motion of the camera as in (3.3), in which part of the feature velocity is given by the (unknown) target velocity $\dot{s}_o = \mathbf{L}_s^c \mathbf{v}_o$.

To estimate the target motion we now describe two instances of an EKF for the cases of image points and 3D pose error as visual features respectively. Note that both filters could be used in a classical VS control scheme to compensate for the target motion while here we exploit the entire state of the filters to implement the control law (5.21) for (i) virtually increasing the visual measurements from the camera (by running the filter at much higher frequency than the camera itself), and for (ii) compensating for the target own motion and, thus, significantly reducing the tracking error.
### 5.5.1 EKF for IBVS with image points features

The goal of the filter is to both increase the information rate of the camera and to estimate the velocity of the moving target. The feature vector $\mathbf{s}$ will then be part of the filter state whose dynamics following the visual-servoing kinematic model (3.19). For each image point feature $f_i = (f_{x_i}, f_{y_i})$, $\forall i = 1, \dots, 4$, the associated interaction matrix (3.7) (see Section 3.3.1) requires the knowledge of the depth of the point Z which is also estimated by the filter as long as the trajectories are “exciting” enough. The depth dynamic is given by [74]

$$ \dot{Z} = L_Z(f, Z)^c \mathbf{v} = \begin{bmatrix} 0 & 0 & -1 & -f_y Z & f_x Z & 0 \end{bmatrix}^c \mathbf{v}. \quad (5.22) $$

Furthermore, to implement controller (5.21), the feature acceleration contribution due to the motion of the target ($\ddot{\mathbf{s}}_o$) is also needed. The continuous-time state of the filter, stacking here four image points ($\mathbf{s} \in \mathbb{R}^8$), is then $\mathbf{x}(t) = [\mathbf{s}(t) \ \mathbf{z}(t) \ \dot{\mathbf{s}}_o(t) \ \ddot{\mathbf{s}}_o(t)]^\top = [\mathbf{x}_1(t) \ \mathbf{x}_2(t) \ \mathbf{x}_3(t) \ \mathbf{x}_4(t)]^\top \in \mathbb{R}^{28}$, and explicitly the filter dynamics $\dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t), \mathbf{u}(t)) + \mathbf{w}_t$ one has:

$$ \dot{\mathbf{x}} = \begin{bmatrix} L_s(\mathbf{x}_1, \mathbf{x}_2)^c \mathbb{T}_e^e \mathbf{J}_e \mathbf{u} - \mathbf{x}_3 \\ L_Z(\mathbf{x}_1, \mathbf{x}_2)^c \mathbb{T}_e^e \mathbf{J}_e \mathbf{u} - L_s^\dagger(\mathbf{x}_1, \mathbf{x}_2)\mathbf{x}_3 \\ \mathbf{x}_4 \\ 0 \end{bmatrix} + \mathbf{w}_t \quad (5.23) $$

in which $\mathbf{u}(t) = \dot{\mathbf{q}}(t)$ is the plant input and $\mathbf{w}_t = [\mathbf{w}_{1t}^\top \ \mathbf{w}_{2t}^\top \ \mathbf{w}_{3t}^\top \ \mathbf{w}_{4t}^\top]^\top \in \mathbb{R}^{28}$ is a vector of additive Gaussian noise $\mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)$. In the general case one does not have a model of the target motion, therefore we assume the dynamics of the acceleration of the features due to the target motion $\ddot{\mathbf{s}}_o(t)$ to be approximately constant (corrupted by some added noise), i.e., $\ddot{\mathbf{s}}_o(t) = \mathbf{w}_{4t}$. Clearly if an accurate motion model was available, it could be used, adjusting opportunely the filter dynamics.

To update the process state estimate with the dynamics and its uncertainty, represented by the covariance matrix $\mathbf{P}_k$, we firstly need to linearize and discretize it at the control period $\Delta T$:

$$ \bar{\mathbf{A}} = \left. \frac{\partial \mathbf{f}(\mathbf{x}, \mathbf{u})}{\partial \mathbf{x}} \right|_{\mathbf{x}=\mathbf{x}_k, \mathbf{u}=\mathbf{u}_k} \quad (5.24) $$

$$ \mathbf{A}_k = e^{\bar{\mathbf{A}}\Delta T} \approx \mathbb{I} + \bar{\mathbf{A}}\Delta T \quad (5.25) $$

$$ \mathbf{x}_{k+1|k} = \mathbf{x}_{k|k} + \mathbf{f}(\mathbf{x}_{k|k}, \mathbf{u}_k)\Delta T \quad (5.26) $$

$$ \mathbf{P}_{k+1|k} = \mathbf{A}_k \mathbf{P}_{k|k} \mathbf{A}_k^\top + \mathbf{Q}_k \quad (5.27) $$

with $\mathbf{u}_k = \mathbf{u}(\tau), \forall \tau \in [k\Delta T, (k+1)\Delta T)$.
The state estimation improves during the measurement update phase based on the measures $y_k$. In our case, the measurement is the set of image point features in the image plane, leading then to a linear measurement equation in the filter state that is already in discrete time:

$$ \eta_{k+1} = s_{k+1} + W_{s_{k+1}} = C_{k+1}x_{k+1|k} + W_{s_{k+1}} \quad (5.28) $$

with $W_{s_{k+1}}$ a vector of discrete-time white noise with auto-correlation matrix $R_{k+1} \in \mathbb{R}^{8 \times 8}$, while $C_{k+1} = [\mathbb{I}_8 \ O_{8 \times 20}]$ is the measurement sensitivity matrix. It then follows the measurement update phase:

$$ S_{k+1} = (C_{k+1}P_{k+1|k}C_{k+1}^{\top} + R_{k+1}) \quad (5.29) $$

$$ K_{k+1} = P_{k+1|k}C_{k+1}^{\top}S_{k+1}^{-1} \quad (5.30) $$

$$ x_{k+1|k+1} = x_{k+1|k} + K_{k+1}(y_k - C_{k+1}x_{k+1|k}) \quad (5.31) $$

$$ P_{k+1|k+1} = (I - K_{k+1}C_{k+1})P_{k+1|k} \quad (5.32) $$

### 5.5.2 EKF for PBVS

The filter state for Pose-Based Visual Servoing (PBVS) is very similar to the previous case but without the depth dynamics. The feature $s$ is a pose error vector whose rotational part is represented with the axis/angle representation and the filter state is now $x(t) = [s(t) \dot{s}_o(t) \ddot{s}_o(t)]^\top = [x_1(t) \ x_2(t) \ x_3(t)]^\top \in \mathbb{R}^{18}$ with dynamics

$$ \dot{x} = \begin{bmatrix} L_s(x_1)^c \mathbb{T}_e^e J_e u - x_2 \\ x_3 \\ 0 \end{bmatrix} + w_t. \quad (5.33) $$

In order to update the state estimate with the process equations, one can follow the same steps as in (5.24)–(5.27).

The “measurements” provided by the camera in the PBVS case is the relative pose of the observed object w.r.t. the camera frame, expressed as the homogeneous transformation matrix $^cT_o(y_k)$. This leads to a discrete-time, non-linear and implicit measurement equation w.r.t. both the measure and filter state:

$$ \eta_{k+1} = h(x_{k+1|k}, y_{k+1}). \quad (5.34) $$

The measurement equation (5.34) is a constraint equation ($h(x, y) = 0$) and to compute it, we first define the “closure” equation: $^cT_c(x_{k+1|k}) \ ^cT_o(y_{k+1}) \ ^cT_o^{-1} = I$, being $^cT_o$ the desired pose of the camera with respect to the object (which is of course known by design. See Figure 5.1). The pose vector associated to the left-hand side of the closure equation must be zero, and this relationship represents
the measurement equation of the form (5.34). The measurement update equations differ from those in (5.29)–(5.32) as follows:

$$H_{k+1} = \frac{\partial h(x, y)}{\partial x} \Big|_{x=x_{k+1|k}, y=y_{k+1}} \qquad (5.35)$$

$$D_{k+1} = \frac{\partial h(x, y)}{\partial y} \Big|_{x=x_{k+1|k}, y=y_{k+1}} \qquad (5.36)$$

$$S_{k+1} = (H_{k+1} P_{k+1|k} H_{k+1}^{\top} + D_{k+1} R_{k+1} D_{k+1}^{\top}) \qquad (5.37)$$

$$K_{k+1} = -P_{k+1|k}H_{k+1}^{\top}S_{k+1}^{-1} \qquad (5.38)$$

$$x_{k+1|k+1} = x_{k+1|k} + \eta_{k+1}h(x_k, y_k) \qquad (5.39)$$

$$P_{k+1|k+1} = P_{k+1|k} + K_k H_k P_{k+1|k} \qquad (5.40)$$

Once the filter converges, one can obtain the target velocity from the estimated disturbance state as $c\mathbf{v}_o(t_k) = L_s^{-1}\mathbf{x}_3(t_k)$, as we similarly did using $L_s^\dagger$ in the depth dynamics of the EKF for IBVS (see second component of (5.23)).

## 5.6 Simulations

In order to validate the tracking performance of the proposed EKFs and study the feasibility of using second-order visual servoing in a task involving contacts with the environment, we simulate a Peg-in-Hole task with a moving target using FrankaSim [8], a co-simulation environment implemented in ViSP [10] and CopeliaSim [75]. With FrankaSim it is possible to carry out dynamic simulations with a Panda robot and process different sensor information. This simulator will be further detailed in Chapter 8. The simulation was performed in synchronous mode with a time step of 1 ms using Vortex physics engine, since it is capable to deal with non-convex shapes. A vision sensor was mounted on the robot wrist and we programmatically drop and delay the grabbed frames in order to simulate a 30 fps (33 ms) camera rate with 10 ms of feature extraction delay. The visual features are extracted from an AprilTag of the family 36h11 using ViSP's detector (AprilTag pose or its corners). A 5 × 5 Gaussian mask is applied on the grabbed images to smooth the AprilTag's borders making the detection process more realistic. This is a convenient way to simulate noise because, despite being easy to add Gaussian noise around the detected pixel of points features, the uncertainties on the estimated pose depend on the distance of the object from the camera [76]. White noise of the magnitude of the real robot was also added to the measured joint velocities $\dot{\mathbf{q}}$ with standard deviation $\sigma = 0.021$ rad/s. The robot control loop runs at 1 kHz as for the real one.
### 5.6.1 Task description and controllers implementation

The simulated task consists in inserting the peg held by the manipulator's gripper into the hole of a work-piece both of cylindrical shape with diameters of 9 and 10 mm respectively. The piece is placed on a moving platform whose lowest joints turn at 0.5 rad/s in opposite directions allowing the upper part to undergo a purely translational motion, while the last joint of the platform performs a sinusoidal velocity motion of amplitude 0.2 rad/s and frequency $1/\pi$ Hz (see Fig. 5.3 and video).

<figure><img src="image_28.png" /><figcaption>**Figure 5.3** – Simulation setup: The Panda robot is in the initial configuration of the experiment. World and base $\Sigma_w \equiv \Sigma_b$, camera $\Sigma_c$, end-effector $\Sigma_e$ and target $\Sigma_o$ frames are drawn. An AprilTag is attached on the workpiece and tracked by the camera. The lower plate of the platform rotates at $\omega_1 = 0.5$ rad/s, the second plate at $\omega_2 = -\omega_1$ while the last joint motion if $f(t) = 0.2\sin(2t)$.</figcaption></figure>

Although the task constrains all the 6 degrees of freedom (DoF) of the operational space, our robot is a redundant manipulator and to prevent the robot joints to move along directions lying in the kernel of the task Jacobian $J_s$, the null-space directions must be damped or, in general, a suitable secondary task must be considered. Controller (5.21) is then modified into a more computationally efficient form with the addition of a damping term in the null space of the task:

$$ u_{\tau} = (J_s B(q)^{-1})^{\dagger} \left( \ddot{s}^d + D_s(\dot{s}^d - L_s c \mathbb{T}_e^e J_e \dot{q} - \dot{s}_o) + K_s(s^d - s) + \ddot{s}_o - h_q \right) + c(q, \dot{q}) + P^{\perp} \tau_N \quad (5.41) $$

in which the gravity term does not appear anymore since it is already compensated by the robot (in both the real and simulated cases), and $P^{\perp}$ is a Null-space
projector that applies the secondary damping torques $\tau_N = -k_d \dot{q}$ on ker($\mathbf{J}_s \mathbf{B}^{-1}$). Assuming the manipulator is in a non-singular configuration, in the PBVS case the task Jacobian has dimension $6 \times 7$ with $\text{rank}(\mathbf{J}_s \mathbf{B}^{-1}) = 6$ and a Null-space projector based on the dynamically consistent Inertia weighted pseudo-inverse $\mathbf{P}^{\perp} = (\mathbb{I} - \mathbf{J}_s^{\top} \bar{\mathbf{J}}_s^{\top})$ can be computed [11], where $\bar{\mathbf{J}}_s = \mathbf{B}^{-1} \mathbf{J}_s^{\top} (\mathbf{J}_s \mathbf{B}^{-1} \mathbf{J}_s^{\top})^{-1}$. On the other hand, in the IBVS case the task Jacobian is $8 \times 7$ with $\text{rank}(\mathbf{J}_s \mathbf{B}^{-1}) = 6$ and more general methods must be used to find an orthogonal basis of ker($\mathbf{J}_s \mathbf{B}^{-1}$), e.g., resorting to a Singular Value Decomposition (SVD) [77].

### 5.6.2 Simulation Results

We ran several simulations for both proposed filters in which the estimates were used in the controller (5.41) to accomplish the task described in Section 5.6.1.

During the first trials we noticed that the target's velocity estimation already showed a good performance but it was not perfect due to the low camera rate and the lack of a motion model of the target. A small tracking error remains, so we added an integral term which is activated when the feature error is sufficiently small in order to compensate for any final error and correctly realize the insertion task. The new controller is then

$$e_{s_{int}} = \begin{cases} \mathbf{K}_I \int (\mathbf{s}^d - \mathbf{s}) dt & \text{if } \|\mathbf{s}^d - \mathbf{s}\| \le \epsilon \\ \mathbf{0} & \text{elsewhere} \end{cases}$$

$$\mathbf{u} = (\mathbf{J}_s \mathbf{B}(\mathbf{q})^{-1})^{\dagger} \left( \ddot{\mathbf{s}}^d + \ddot{\mathbf{s}}_o - \mathbf{h}_q + \mathbf{D}_s (\dot{\mathbf{s}}^d - \mathbf{L}_s^c \mathbb{T}_e^e \mathbf{J}_e \dot{\mathbf{q}} + \dot{\mathbf{s}}_o) + \mathbf{K}_s (\mathbf{s}^d - \mathbf{s}) + e_{s_{int}} \right) + \mathbf{c}(\mathbf{q}, \dot{\mathbf{q}}) + \mathbf{P}^{\perp} \tau_N + \mathbf{u}_{\tau}(0) e^{-\mu t} \quad (5.42)$$

Since for the real robot it is recommended to generate the torque commands starting from zero (recall that gravity is internally compensated) we added a rapidly decaying exponential term, whose initial value coincides with the torque command at time $t = 0$, to guarantee a smooth start of the robot with a torque command that starts from zero ($\mathbf{u}(0) = 0$).

The following experiment for the IBVS case was carried out using controller (5.42) with controller gains: $\mathbf{K}_s = \text{diag}(250) \text{s}^{-2}$, $\mathbf{D}_s = \text{diag}(50) \text{s}^{-1}$, $\mathbf{K}_I = \text{diag}(300) \text{s}^{-2}$ and $k_d = 20 \text{ kg.m.s}^{-2}$ and filter parameters $\mathbf{R}_{k+1} = \text{diag}(6.8e^{-06})$, $\mathbf{Q}_{k+1} = \text{diag}(1e^{-06}\mathbb{I}_8, 1e^{-06}\mathbb{I}_4, 5e^{-04}\mathbb{I}_8, 1e^{-06}\mathbb{I}_8)$. The filter state was then initialized with the position values extracted at the first grabbed image, while
the depths were initialized at 0.2 m and considering the target starts at rest
$x(0) = [\mathbf{s}(0) \ \mathbf{z}(0) \ \dot{\mathbf{s}}_o(0) \ \ddot{\mathbf{s}}_o(0)]^\top = [s(0) \ 0.2 \ 0 \ 0]^\top$.

The filter shows an excellent feature tracking and a good reconstruction of the estimated target's velocity (see Figure 5.4) once the depth estimation stabilizes. As we can see from Figure 5.5, at the beginning of the experiment, in which the camera is far and approaches the target, the motion is sufficiently exciting to reconstruct the depth's dynamics leading it to converge towards its true value but then (after 1 s of experiment) when the camera is on top of the target, only the small motion of the last joint of the turntable excites this dynamics. The depth estimates remain almost constant for the rest of the experiment without further converging towards the true values.

<figure><img src="image_29.png" /><figcaption>**Figure 5.4** – Peg-in-Hole simulation by IBVS: (a): Initial configuration and camera view. (b): Final configuration and camera view; the features paths in the image plane are visible in green while the path followed by the camera is sketched in blue. (c): Target’s linear (top) and angular (bottom) velocity estimated vs Ground-Truth. (d): Estimated vs Ground-Truth features’ positions (top) and features’ error ($e_s = s^d - s$).</figcaption></figure>

While the depth estimation is converging (between 0 and 1 s) the reconstruc-
tion of the target velocity is affected causing the controller to badly compensate
for this velocity making the feature error to increase. Despite the sensibility of
the controller in regards of the feature's depth, it was capable of successfully com-
pleting the task - thanks to its ability in reconstructing the feature's depth under
the hypothesis of a sufficiently exciting motion - but this is not always the case,