Kineto-Static Methods for Whole-Robot Interaction Control

Università degli Studi di Pisa

FACOLTÀ DI INGEGNERIA

DIPARTIMENTO DI INGEGNERIA DELL’INFORMAZIONE

Corso di Dottorato in

Automatica, Robotica e Bioingegneria

Scuola di Dottorato in Ingegneria

"Leonardo da Vinci"

SSD: ING-INF/04

Ph.D. Dissertation on

Kineto-Static Methods for Whole-Body

Robot Interactions

Candidate Student:

Edoardo Farnioli

Supervisors:

Prof. Antonio Bicchi

Abstract

In robotic grasping, it is usual the distinction between tip grasp and whole-hand grasp. In the first case, only fingertips interact with the grasped object, as it happens grasping e.g. a strawberry. In the sec-ond case, on the contrary, also internal phalanges are in contact with the object, as it happens, for example, grabbing an hummer from the handle.

Similarly, it is usual for a humanoid robot to interact with the en-vironment with the feet and the hands. But the interaction can also occur in the internal limbs, as the torso or the knees, as e.g. lifting up a large box. We refer to these cases as whole-body loco-manipulation tasks.

In both the cases, collectively referred to as whole-body robot inter-actions, multiple contacts, can greatly affects the robot capabilities. As a consequence, the presence of a high number of degrees of free-dom is not sufficient to ensure the full controllability of the interaction forces and/or of the system displacements.

This thesis presents novel thinking and approaches to the kineto-static analysis of whole-body robot interaction, general enough to treat compliant and/or underactuated robots.

system is obtained considering the congruence and the equilibrium equations, both for the grasped object and the robotic hand. The constitutive equations of the contact are introduced via a penalty for-mulation, making the problem of the contact force computation stat-ically determined.

Grouping together all the previous equations, the Fundamental Grasp Equation (FGE) is obtained. The FGE is investigated act-ing both on (i) its coefficient matrix, the Fundamental Grasp Matrix (FGM) and on (ii) the solution space of the system, i.e. the nullspace of the FGM. From the elaboration of the FGM, the canonical form of the Fundamental Grasp Equation (cFGE) is obtained, both in numeric and symbolic form. The cFGE provides some relevant information on the system as the subspace of the controllable internal forces, the sub-space of the controllable object displacements, and the grasp compli-ance matrix. Moreover, some relevant manipulation tasks are defined in terms of nullity or non-nullity of the system variables (joint angles, joint torques, object displacements, etc...), as e.g. the pure squeeze of the object, in which the contact forces change without affecting the object configuration, and the kinematic grasp displacements in which the object is moved without influencing the contact forces. In order to discover the feasibility of such predefined tasks, a method for investigating the solution space of the FGE is presented, based on the computation of the reduced row echelon form (RREF) of suitable matrices.

Some of the previous methods have been used to support the de-sign of two prototypes: the Velvet Finger and the Pisa/IIT SoftHand. The Velvet Finger is a smart gripper, equipped with conveyor belts on the surfaces of its phalanges. In this case, the decomposition of the solution space of the FGE was used to extract the necessary

informa-tion for evaluating the manipulability capabilities of the prototype, showing the convenience of the actuation design with respect to other more conventional solutions. The Pisa/IIT SoftHand is a humanoid robotic hand in which the human inspired principle of the synergistic underactuation is implemented via the adaptive synergy mechanism. The construction of the cFGE for both the cases brings to find the conditions for which the two underactuation methods are equivalent in terms of controllable forces and displacements.

The first part of the thesis is completed by the definition of the reduced form of the Fundamental Grasp Equation (rFGE), obtained substituting all the congruence equations into the others. As a con-sequence, the rFGE can be seen as the first order approximation of a suitable system of nonlinear equations, called the Equilibrium Man-ifold (EM) of the system. The EM formulation was exploited to approach the problem of regulating the grasp compliance in the non trivial case of a robotic hand equipped with variable stiffness actua-tors (VSA) and synergistic underactuation.

In the second part of the thesis, the previous concepts are used to study compliant humanoid robots in whole-body loco-manipulation tasks. The introduction of the quasi-static form of the congruence, the equilibrium and the constitutive equations of the system allows to define the Fundamental Loco-Manipulation Equation (FLME). After the canonical form of the Fundamental Loco-Manipulation Equation (cFLME) is found, some relevant information can be extracted as the subspace of the controllable contact forces and the controllable displacements of the center of mass of the robot. Relevant loco-manipulation tasks are later defined in terms of (non-)nullity of the system variables. Moreover, the basis of the controllable contact forces is used to show that the contact force distribution problem can be for-mulated in convex fashion.

grasp-tion Equagrasp-tions (FWE). The FWE shows that grasping and loco-manipulation problems can share not just analysis methods, but also part of the analysis results, e.g. the symbolic expressions of the blocks composing the respective canonical forms of the Fundamental Equa-tions, for equivalent actuation conditions.

Acknowledgement

I would like to greatly thank my supervisors Prof. Antonio Bicchi, and Prof. Marco Gabiccini for have been an irreplaceable guide, during my doctoral studies, for having led me toward a rigorous approach, and for having helped to give my best in every moment. I would like to greatly thank also the Centro di ricerca E. Piaggio, University of Pisa, and the Department Advanced Robotics, Italian Institute of Technol-ogy, for having hosted me in such inspiring environment, provided me the possibility to work with such brilliant students, researchers and Professors. I would like to thank in particular Prof. Darwin Cald-well and Dr. Nikos Tsagarakis, from IIT, and Prof. Lucia Pallottino, from University of Pisa, for their greatly helpful advice and insights, and for the fruitful discussions. I would like to thank the European projects The Hand Embodied [P22], Hands.DVI [P23], Roblog [P24], Softhands [P25], PaCMan [P26], and Walkman [27], for the generous funding of my work.

There is also a number of colleagues that I really would like to thank for their enjoyable and extremely productive collaboration. With no pretense of order and completeness, I would like to thank Manuel Bonilla, incomparable companion of adventures (and beers), Manuel Catalano, an infinite natural force, Giorgio Grioli, Matteo Bianchi, Manolo Garabini and Arash Ajoudani, fixed reference points during this path.

A infinite quantity of special thanks to my love Irene for being the brightest ray of sunshine of my life.

Contents

Underactuated Robotic Hands . . . 9 1.3.2 Outline of part II:

Compliant Humanoid Robots . . . 12

I

Underactuated Robotic Hands

15

2 Grasp Analysis Tools for Synergistic Underactuated

Robotic Hands 17

2.1 Introduction . . . 18 2.2 Human-Inspired Underactuation . . . 20

2.3 Quasi-Static Model of the System . . . 22

2.3.1 Equilibrium Equation of the Object . . . 25

2.3.2 Congruence Equation of the Object . . . 26

2.3.3 Congruence Equation of the Hand . . . 27

2.3.4 Equilibrium Equation of the Hand . . . 28

2.3.5 Hand/Object Interaction Model . . . 29

2.3.6 Joint Actuation Model . . . 31

2.3.7 Underactuation of the Hand . . . 31

2.3.8 The Fundamental Grasp Matrix (FGM) . . . . 32

2.4 Controllable System Perturbations . . . 34

2.4.1 Find-X Method . . . 35

2.4.2 GEROME-B: Gauss Elementary Row Operation Method for Block Partitioned Matrices . . . 36

2.4.3 The Canonical Form of the Fundamental Grasp Matrix (cFGM) . . . 41

2.5 Relevant Properties of the cFGM . . . 43

2.5.1 Controllable Internal Forces . . . 43

2.5.2 Contact Force Transmission Caused by an Ex-ternal Wrench . . . 44

2.5.3 Controllable Internal Object Displacements . . 44

2.5.4 Grasp Compliance . . . 44

2.6 Main Categories of Manipulation Tasks . . . 45

2.6.1 Internal Forces and Displacements . . . 45

2.6.2 External Forces and Displacements . . . 46

2.6.3 Pure Squeeze . . . 46

2.6.4 Spurious Squeeze . . . 46

2.6.5 Kinematic Grasp Displacements . . . 46

2.6.6 Redundant Motion of the Hand . . . 47

2.6.7 External Structural Forces . . . 48 2.6.8 Resultant Decomposition of the Solution Space 48

2.7 Block Decomposition of the Nullspace Matrix . . . 50

2.8 Complete Taxonomy of the Grasp . . . 52

2.8.1 In-Level Constraints . . . 53

2.8.2 Extra-Level Constraints . . . 56

2.8.3 The Taxonomic Labyrinth . . . 58

2.9 Other Types of (Under-)Actuation . . . 60

2.10 Numerical Examples . . . 62

2.10.1 Precision Grasp . . . 62

2.10.2 Power Grasp . . . 69

2.11 Conclusions . . . 74

3 Tools to Support Robotic Hand Design 77 3.1 Introduction . . . 78

3.2 Velvet Finger . . . 80

3.2.1 The Velvet Finger Prototype . . . 86

3.2.2 Kinematic Design for Grasping Closure . . . 89

3.2.3 Dexterous Under-actuation . . . 93

3.2.4 Manipulability Analysis . . . 95

3.2.5 Experimental Validation . . . 100

3.3 The Pisa/IIT SoftHand . . . 106

3.3.1 Fully Actuated Hands . . . 108

3.3.2 Soft Synergies . . . 112

3.3.3 Adaptive Synergies . . . 116

3.3.4 From Soft to Adaptive Synergies . . . 119

3.3.5 The Pisa/IIT SoftHand Prototype . . . 122

3.3.6 Experimental Results . . . 127

3.4 A New Set of Possibilities: Grasping with SoftHands . 135 3.4.1 Simulator Implementation . . . 140

3.4.2 Batch Simulation Setup . . . 142

3.4.3 Simulation Results . . . 145

4 Non-Local Kineto-Static Methods for Grasping 155

4.1 Introduction . . . 156

4.2 Compliance Regulation on the Equilibrium Manifold . 157 4.3 Problem Statement . . . 159

4.4 Equilibrium Manifold of the Grasp . . . 160

4.4.1 Equilibrium of the Object . . . 160

4.4.2 Equilibrium of the Hand . . . 162

4.4.3 Equilibrium of the Contact Virtual Springs . . . 162

4.4.4 Equilibrium of the Elastic Joints . . . 163

4.4.5 Equilibrium of the Synergistic Underactuation . 163 4.4.6 The Equilibrium Manifold . . . 164

4.5 Tangent Space of the Equilibrium Manifold . . . 164

4.5.1 Perturbation of the Object Equilibrium . . . 165

4.5.2 Perturbation of the Hand Equilibrium . . . 165

4.5.3 Perturbation of the Contact Springs . . . 165

4.5.4 Perturbation of the Elastic Joints . . . 166

4.5.5 Perturbation of the Synergistic Underactuation 167 4.5.6 The reduced form of the Fundamental Grasp Equation and the Grasp Compliance Matrix . . 167

4.6 Grasp Compliance Regulation Algorithm . . . 170

4.7 Numerical Examples . . . 172

4.7.1 Varying Joint Stiffness . . . 174

4.7.2 Varying Synergy Variables . . . 175

4.7.3 Varying Joint Stiffness and Synergy Variables . 176 4.7.4 Grasp Compliance Regulation with the DLR/HIT hand II . . . 178

4.8 SoftPlanning on the Equilibrium Manifold . . . 179

4.8.1 Planning with Closed Kinematic Chains . . . . 180

4.9 Motion Planning Problem of Systems Under Constraints183 4.9.1 Relaxing Constraints . . . 184

4.10.1 Biased Random Sampling . . . 186

4.10.2 Exploiting the Equilibrium Manifold . . . 188

4.11 Execution of the Planned Path . . . 190

4.11.1 Control . . . 190

4.12 Simulations . . . 191

4.13 Conclusions . . . 194

II

Compliant Humanoid Robots

197

5.2 Whole-Body Loco-Manipulation Tasks for Humanoid Robots . . . 201

5.3 System Modeling . . . 203

5.3.1 Reference Frames . . . 203

5.3.2 Congruence Equations . . . 207

5.3.3 Displacement of the CoM . . . 209

5.3.4 Equilibrium of the System . . . 210

5.3.5 Quasi-Static Formulation of the Equilibrium Equa-tions . . . 212

5.3.6 Elastic Joint Model . . . 212

5.4 The Fundamental Loco-Manipulation Matrix . . . 213

5.4.1 Canonical Form of the FLMM . . . 214

5.4.2 Relevant Properties of the cFLMM . . . 216

5.5 Whole-Body Loco-Manipulation Task Feasibility . . . . 216

5.5.1 Internal Forces and Displacements . . . 217

5.5.2 External Forces and Displacements . . . 217

5.5.3 Redundant Motion of the Robot . . . 218

5.5.5 Body Movement Preserving the Center of Mass 219

5.5.6 External Structural Wrench on the Body . . . . 219

5.5.7 External Structural Wrenches on the Free End-Effectors . . . 219

5.6 Contact Force Optimization . . . 220

5.7 Numerical Examples . . . 224

5.7.1 Pushing Humanoid Example . . . 224

5.7.2 Humanoid Robot Approaching a Slope . . . 226

5.8 Experimental Results . . . 230

5.8.1 Balancing on Flat Terrain . . . 234

5.8.2 Balancing on a Partially Uneven Terrain . . . . 237

5.8.3 Balancing on Uneven Terrain (1/2) . . . 240

5.8.4 Balancing on Uneven Terrain (2/2) . . . 242

5.8.5 Discussion on the Experimental Results . . . 244

5.9 Conclusions . . . 244

6 On the Connections Between Grasping and Loco-Manipulation 247 6.1 Introduction . . . 247

6.2 A Brief Review of Grasp Analysis . . . 249

6.3 A Brief Review of Whole-Body Loco-Manipulation Anal-ysis . . . 250

6.4 Similarities and Differences Between FGE and FLME . 251 6.4.1 Similiraties . . . 251

6.4.2 Differences . . . 253

6.5 One Equation for Two Problems . . . 254

6.5.1 Extensions of the Basic Form of FWBE . . . 258

6.6 Conclusions . . . 260

Conclusions

261

Appendix

264

A Homogeneous Transformation and Adjoint Operators265

B Spatial Jacobian Matrix 269

C Derivative Terms of the Jacobian Matrix 271

C.1 Derivative of the Jacobian matrix with respect to the hand configuration . . . 271 C.2 Derivative of the Jacobian matrix with respect to the

object configuration . . . 273 D Block Elements of the Canonical Form of the

Funda-mental Grasp Matrix 277

D.1 Fundamental Grasp Matrix for a Fully Actuated Hand Grasping an Object . . . 277 D.2 Fundamental Grasp Matrix for a Fully Actuated Hand

with Elastic Joints Grasping an Object . . . 279 D.3 Fundamental Grasp Matrix for a Synergistic

Underac-tuated Hand Grasping an Object . . . 282

List of Figures

1.1 The Walkman robot developed by IIT. . . 2

1.2 Examples of whole-body robot interactions. . . 3

1.3 Sometimes the difference between grasping and loco-motion is not so evident. . . 5

2.1 Compliant grasp by an underactuated robotic hand. . . 23

2.2 Complete taxonomy of the grasp. . . 59

2.3 Grasp of a circular object by a two-fingered robotic hand. 63 2.4 Graphic representations of numerical results for the in-ternal precision grasp variations. . . 67

2.5 Grasp of a square by a two fingered spider-like hand. . 69

2.6 Graphic representations of numerical results for the in-ternal power grasp variations. . . 72

3.1 The Velvet Fingers smart gripper. . . 81

3.2 Cut view of the Velvet Finger prototype. . . 88

3.3 Closure movement of the Velvet Finger. . . 89

3.4 Sketch of the kinematic of the Velvet Finger. . . 90

3.5 Velvet Finger closing with an object int it. . . 91

3.7 Advantages of friction modulation. . . 94

3.8 Controlled slipping between active surfaces and grasped object. . . 94

3.9 Manipulability ellipsoids for different number of actu-ators, without using conveyor belts. . . 98

3.10 Manipulability ellipsoids for different number of actu-ators, using conveyor belts. . . 99

3.11 Velvet Finger closing around a spherical object. . . 100

3.12 Grasping with touching fingertips. . . 101

3.13 Velvet Finger grasping several objects. . . 102

3.14 In-hand object roto-tranlsation. . . 103

3.15 Different grasp configurations of a cube. . . 104

3.16 Lift of a box. . . 105

3.17 Pisa/IIT SoftHand Prototype . . . 107

3.18 Schematics of a simple fully actuated hand grasping an object. . . 108

3.19 Hard Synergy actuation scheme. . . 111

3.20 Variable stiffness actuation scheme. . . 114

3.21 Soft synergy actuation scheme. . . 115

3.22 Adaptive Under-Actuation scheme. . . 117

3.23 Adaptive synergies underactuation scheme. . . 118

3.24 Kinematics of the Pisa/IIT SoftHand. . . 123

3.25 Main components of the Pisa/IIT SoftHand . . . 124

3.26 Rolling contact joints. . . 125

3.27 Rolling contact joint in the Pisa/IIT SoftHand . . . 126

3.28 Partially exploded of the Pisa/IIT SoftHand . . . 128

3.29 Robustness test of the joints. . . 128

3.30 Robustness test of the hand. . . 129

3.31 Violent impact test of the hand. . . 130

3.32 Experimental grasps with the Pisa/IIT SoftHand. . . . 131

3.34 Grasps executed with the Pisa/IIT SoftHand mounted

on a Kuka Light Weight Robot. . . 133

3.35 Human interface for using the Pisa/IIT SoftHand. . . . 134

3.36 Grasps executed using the human interface for the hand.134 3.37 Paradigm shift: from rigid to soft manipulation. . . 136

3.38 A human hand grasping a cup with three different ap-proaches (top panels) and the same grasps reproduced with the Pisa/IIT SoftHand (bottom panels). . . 138

3.39 The Pisa/IIT SoftHand mounted on an a human arm. 138 3.40 A person with the arm-mounted SoftHand can seam-lessly execute also difficult manipulation tasks which involve combined interactions between hand, object and environment. . . 139

3.41 Implementation of the rolling-joint in ADAMS . . . 141

3.42 A flow chart of the use of ADAMS and MATLAB for running batch simulations. . . 143

3.43 Sketches of the free closure movement of the Pisa/IIT SoftHand in ADAMS simulation. . . 148

3.44 Examples of stable grasp achieved with the ADAMS simulations. . . 149

3.45 MATLAB representation of hand posture configuration attempt for the cup. . . 150

3.46 A snapshot sequence for the pot, comparing the simu-lation and the experiment results. . . 151

3.47 A snapshot sequence for the colander, comparing the simulation and the experiment results. . . 152

3.48 A possible application of the tool. . . 153

4.1 The DLR/HIT Hand II grasping a sphere. . . 158

4.2 A 2-RRR gripper grasping a sphere. . . 173

4.4 Final configuration using the synergistic variables. . . . 176 4.5 Final configuration varying both the synergistic

vari-ables and the joint stiffness. . . 177 4.6 Initial and final configurations of the DLR/HIT hand

grasping a sphere. . . 179 4.7 Compliant bimanual system in a manipulation task. . . 181 4.8 Differences of motion planning problems with and

with-out constraints. . . 183 4.9 Motion planning problem under relaxed constraints. . . 184 4.10 Graphical explanation of the different sample methods. 187 4.11 Control action on the pushing/pulling forces against

the constraint. . . 189 4.12 Kuka robot constrained the maintain the contact with

a plane. . . 191 4.13 Two finger hand for testing soft-RRT*. . . 192 4.14 Normal contact forces resulting from the planning phase.193 4.15 Final path of the presented test. . . 194 5.1 A humanoid robot engaged in whole-body loco-manipulation

task. . . 204 5.2 A humanoid robot pushing a heavy object in a typical

whole-body loco-manipulation task. . . 225 5.3 A humanoid robot upright in a vertical plane. . . 227 5.4 Optimized configurations varying inclination and

fric-tion condifric-tions on the feet. . . 228 5.5 The COMAN robot. . . 231 5.6 Detail of the environment for the partially uneven

ter-rain test. . . 232 5.7 Detail of the environment for the uneven terrain test. . 233 5.8 Detail of the environment for the uneven terrain test. . 234 5.9 Contact force errors for tests on flat terrain. . . 235

5.10 COMAN robot balancing on flat terrain varying fric-tion condifric-tions. . . 236 5.11 Contact force errors for tests on a partially uneven

ter-rain. . . 238 5.12 COMAN robot balancing on the partially uneven terrain.239 5.13 Contact force errors for tests on a uneven terrain (1/2). 240 5.14 COMAN robot balancing on uneven terrain (1/2). . . . 241 5.15 Contact force errors for tests on a uneven terrain (2/2). 242 5.16 COMAN robot balancing on uneven terrain (2/2). . . . 243 6.1 Find the differences! . . . 248 6.2 Reference scenarios for grasping and loco-manipulation

List of Tables

2.1 Notation for grasp analysis. . . 24 2.2 Object level combinations. . . 53 2.3 Hand/object interface level combinations. . . 54 2.4 Hand level combinations. . . 55 2.5 Synergy level combinations. . . 55 3.1 Simulation results (1/2). . . 146 3.2 Simulation results (2/2). . . 146 5.1 Notation for loco-manipulation analysis (1/2). . . 205 5.2 Notation for loco-manipulation analysis (2/2). . . 206 5.3 Coefficient values for contact constraints. . . 221

List of Algorithms

1 GEROME-B . . . 40 2 Compliance Regulation . . . 171 3 Randomized Planning Algorithm . . . 185

Chapter

1

Introduction

1.1

Motivation

Pomona, California, US, June 6th_{, 2015,}

the DARPA1 _{Robotic Challange (DRC) is just finished and Arati}

Prabhakar, the DARPA Director, claims:

“This is the end of the DARPA Robotics Challenge but only the beginning of a future in which robots can work alongside people to reduce the toll of disasters”.

The scenario of the DRC, one of the most important robot compe-tition of last years, was inspired, indeed, from the Fukushima nuclear disaster, occurred in 2011. An earthquake, followed by a tsunamy, caused serious damages to the nuclear power station. At that time, sending robot inside the power station was tempted, but with little success, motivating the DARPA to organize the challenge.

During the race, many robots showed really good skills addressing such difficult tasks as driving a car, closing a valve and cutting a wall during the race, and between them, it is worth remembering the

Walkman robot, developed by the IIT, showed in Fig. 1.1. Despite this fact, however, the current state of the art of robotics can not ensure a substantial and reliable help in disaster scenarios.

(a) (b)

Figure 1.1: Panel (a) shows the Walkman robot developed by IIT. In Panel (b) the Walkman robot driving a car during the DRC finals.

Among the many skills which a robot needs to move and success-fully act in unstructured (or destructured) scenario, the possibility of overcoming obstacles, balancing on uneven terrain, possibly made slippery by oils or liquids, removing large debris and, in general, ma-nipulating the environment are among the most relevant ones. Situ-ations in which a robot has to grasp a drill or to lift heavy objects, that are expected to be faced in disaster scenarios, share the necessity of properly managing the presence of multiple contacts, not necessar-ily occurring on the extremal parts of the robot. It is quite evident, in fact, that the best way to achieve a stable grasp of a drill, or to firmly use an hummer requires a whole-hand grasp. Similarly, lifting a heavy object is a task that a humanoid robot, as well as human, can

better fulfill with the help of contacts occurring on the torso and on the internal links of the arms, in addition to contacts on its hands.

(a) (b) (c)

Figure 1.2: Examples of whole-body robot interactions. In these situations a full controllability of movements and interaction forces can not be assured.

In such situations, some examples of which are shown in Fig. 1.2, the full controllability of the robot or of the grasped object move-ments, as well as of the contact forces, can not be assured.

Considering the name authoritatively proposed in [28], we will refer to these situations, characterized by the presence of multiple contacts both on the extremities and on the internal limbs, as whole-body robot interactions. About the humanoid robotic field, the limits of the current approaches are described by the authors of [29] as:

“We consider whole-body contact scenarios where multiple extrem-ities of the robot are in stable contact against flat surfaces. In this case, every contact imposes six constraints on the robot’s mobility. We assume that each extremity in contact has enough joints with re-spect to the base link to enable the independent control of its position and orientation. This condition translates into the existence of six or more independent mechanical joints between the base link and the extremity in contact”.

Evidently, this assumption is not valid in whole-body robot inter-action tasks. On the contrary, similar problems have been considered

in grasping and manipulation literature, e.g. for whole-hand manip-ulation [28], [30], [31]. However, the problem of completely under-standing the capabilities of a manipulator interacting with a grasped object has found new interest after the introduction of the human inspired synergistic underactuation.

The high number of degrees of freedom that typically character-ize robotic hands and manipulators bring to some difficulties in con-trol that are in direct conflict with the simplicity with which humans are able to grasp and manipulate. Neuroscience results showed that human control action uses correlated motion of joints, called syner-gies. In order to obtain similar advantages, synergistic control was considered also for robotic hands, proposing again, but for different reasons, the problem of understanding which effects will be produced by the control action, now belonging to a greatly reduced space. De-spite some theoretical results was achieved yet by researchers, the deficiency of analysis tools for studying such complex systems was making slow producing significant results. This thesis moves from kineto-static methods, initially developed for fully actuated robotic hands and arms, and more recently adapted for underactuated hands, with the purpose of extending them and developing new ones, gen-eral enough to allow the study of both underactuated robotic hands and floating base systems, such as humanoid robots, in whole-body interaction tasks.

Although historically there is a division in the literature about the fields, in some circumstances the distinction between locomotion and manipulation becomes thin, or even vanishes, as in the example of Fig. 1.3. A part of this thesis will be dedicated to discuss these cases, which we refer to as loco-manipulation problems, proposing also a unified description for both the problems.

As previously mentioned, this thesis focuses on kineto-static meth-ods. This mean that we will refer to all those situations in which

Figure 1.3: Sometimes the difference between grasping and locomotion is not so evident. Is the hand of the Addams family locomoting or manipulating? Can locomotion and manipulation be treated as a single problem?

dynamic and Coriolis effects are negligible with respect to the others. Similarly to quasi-static transformations in thermodynamic, kineto-static movements of robotic systems bridge together two different equilibrium configurations with a path in which every instantaneous state is an approximation of an equilibrium configuration. Moreover, in continuity with the literature in the field, in this work, with the term quasi-static we will refer to small kineto-static movements, lim-ited to a neighborhood of the initial equilibrium configuration. Even if focusing on kineto-static scenarios can appear as a limitation, actu-ally in many situations it is not. In particular, regarding whole-body robot interaction tasks, as those represented in Fig. 1.2, in many cases the presence of multiple contacts is such to prevent movements where dynamic effects are dominant, making the kineto-static models more descriptive and meaningful, compared to the dynamic counterpart.

1.2

Contributions

This thesis contributes to the areas of modeling of underactuated robotic systems, kinematic design, planning for closed kinematic chains, object level compliance regulation in grasping problems, balancing and pushing problems for humanoid robots. Novel approaches and methods are introduced for modeling whole-body robot interaction problems, finalized to discover and exploit the peculiarities and the capabilities of such systems. A detailed list of the primary contribu-tions of this thesis follows.

Part I: Underactuated Robotic Hands Contributions of Chapter 2

• Review of quasi-static modeling of grasping problem with syner-gistic underactuated robotic hands, also considering derivative terms of the Jacobian matrix, in order to proper analyze con-figurations with contact force preload.

• Introduction of the Fundamental Grasp Equation (FGE) as an essential tool of analysis.

• Presentation of the GEROME-B Algorithm, an extension of the Gauss-Jordan elimination method for operating on block parti-tioned matrices.

• Definition of the canonical form of the Fundamental Grasp Equa-tion (cFGE), providing relevant information on the physical sys-tem, e.g. the matrices spanning the controllable internal forces, the controllable object displacements and the compliance at the

object level. Using GEROME-B these matrices can be also writ-ten in symbolic form, as a function of the main matrices of the initial system (Jacobian matrix, grasp matrix, etc...).

• Definition of the main manipulation tasks in terms of nullity and non-nullity of the system variables.

• Complete taxonomy of feasible manipulation tasks, in terms of nullity and non-nullity of the system variables.

Contributions of Chapter 3

• Using results from Chapter 2 to evaluate manipulability indices of grasping: application to support to the kinematic design of the Velvet Finger smart gripper.

• Using results from Chapter 2 about the FGE and the cFGE for showing how adaptive synergy actuation can imitate a given set of soft synergies: application to support to the kinematic design of the Pisa/IIT SoftHand.

• Discussion about the new set of possibilities that the new re-search line of grasping with SoftHands open, in contrast with previous approaches typically developed for fully actuated hands. The adaptive behavior of the SoftHands can be used to extend the range of graspable shapes, imitating human beings and their ability in exploiting the so called enabling constraints. A ran-domized approach of investigation is also presented to find rel-ative hand/object posture bringing to successful grasps.

Contributions of Chapter 4

• Geometrical interpretation of the reduced form of the FGE, as the description of the tangent plane to the equilibrium manifold of the system.

• Development of an Algorithm able to steer the system toward a new equilibrium configuration with the desired object com-pliance, along a kineto-static path, possibly taking into account both the contribution of variable stiffness actuators and syner-gistic underactuation.

• Development of a planning Algorithm for closed kinematic chains. The equilibrium manifold of the system is considered as sam-pling space. Moreover, results from Chapter 2 are considered to conclude that samples can be taken in the joint configuration space, using later an FGE-based method to recover the values of all the other variables at the equilibrium.

Part II: Compliant Humanoid Robots Contributions of Chapter 5

• Quasi-static model for compliant humanoid robots in whole-body loco-manipulation tasks.

• Definition of the Fundamental Loco-Manipulation Equation (FLME) and of its canonical form (cFLME). Similarly to the cFGE, the cFLME systematically obtained using the GEROME-B Algo-rithm from Chapter 2, provides relevant information on the sys-tem such as the matrices spanning the controllable contact forces and the floating base displacements.

• Formulation of the contact force optimization problem for whole-body loco-manipulation tasks in a convex fashion, considering contact limits such as friction cone and minimum and maximum admissible forces. The proposed formulation is used to face the problems of finding the maximum pushing force against an ob-stacle without slipping, and of balancing on a slippery slope.

Experimental tests with a COMAN robot are also presented for the second case study, for different values of inclination and friction.

Contributions of Chapter 6

• Detailed discussion on similarities and differences between grasp-ing and loco-manipulation problems, based on the characteris-tics of the FGE and of the FLME.

• The two problems are unified under a unique description. As a consequence, this result shows that, over the possibility to ana-lyze both robot hands and humanoids with same tools, also the possibility exists of transferring results between the two prob-lems, as e.g. considerations on dimensions and rank of the re-spective Fundamental matrices, and the symbolic forms of the blocks composing their canonical forms, for equivalent actuation conditions.

1.3

Outline

Following Sections provide some more details on the contributions of this thesis and on the organization of the work, underlining the conceptual connections liking the Chapters.

1.3.1

Outline of Part I:

Underactuated Robotic Hands

Chapter 2presents the main concepts and methods of analysis. The treatise focuses on synergistic underactuated hands, however, the gen-erality of the proposed method allows the study also of others kinds

of (under)actuation. In Part II, same tools will be used also for an-alyzing compliant humanoid robots in whole-body loco-manipulation tasks. This Chapter is the base of all the other Chapters composing this thesis.

A review of the quasi-static model for grasping is presented, based on the screw theory. The mathematical model, considering also the derivative terms of the Jacobian matrix in order to admit the analysis of preload configurations, takes into account synergistic underactua-tion as well as compliance at different levels. The whole system of equations is considered to compose the Fundamental Grasp Equation (FGE), that appears as a linear and homogeneous system of equa-tions. Particular attention is dedicated to elaborating its coefficient matrix, the Fundamental Grasp Matrix (FGM). An Algorithm called GEROME-B, based on an extension of the Gauss-Jordan elimina-tion method able to act on block partielimina-tioned matrices, is presented. Thanks to this, the canonical form of the Fundamental Grasp Equa-tion (cFGE) can be found, which describes a map between the vari-ation of the independent and the dependent variables of the system. From these relationships it becomes evident the physical meaning of some blocks composing the canonical form. The map between the joint displacements and the contact force variation, for example, de-scribes the controllable internal forces, and the map between the ex-ternal wrench and the consequent object displacement describes the grasp compliance. Moreover, after the definition of some relevant ma-nipulation tasks as suitable combinations of null or non-null system variables, a method to discover their feasibility is described, investi-gating the solution space of the FGE. The search of feasible solutions was carried on, considering the FGE as a feasibility constraint for (non-)nullity patterns of variables, until arriving to a complete tax-onomy of the manipulation tasks.

used to support the kinematic design of grippers and robotic hands. In the Velvet Finger prototype, a smart gripper characterized by the presence of active contact surfaces, the decomposition of the solution space of the FGE was used to extract the necessary information for evaluating a proper manipulability index, showing a relevant increase of the manipulability ellipsoid dimensions for the proposed actuation solution with respect to other more conventional designs. In the sec-ond part of the Chapter, the Pisa/IIT SoftHand is presented. The prototype is characterized by a particular kind of underactuation, called adaptive synergy, which allows the hand to adapt to the partic-ular shape is going to grasp. In this case, the cFGE was used to show the possibility to replicate with an adaptive synergy the behavior of the human-inspired soft synergy underactuation. Note that the me-chanical design of the prototypes is not part of the work developed by the author of this thesis. However, some details are anyway presented for the reader convenience.

In Chapter 4 we observe that the FGE can be rewritten in a reduced form (rFGE) in which all the equations are a first order approxima-tion of a more general non linear equaapproxima-tion. As consequence the rFGE can be considered as the analytical expression of the tangent plane to a manifold describing all the equilibrium configurations of the system, called equilibrium manifold (EM). Both the FGE and the EM were used to develop an Algorithm able to move the system along a path composed by a sequence of equilibrium states, toward a new equilib-rium configuration characterized by a given grasp compliance. The method is general enough to treat robotic manipulators in presence of variable stiffness actuators and synergistic underactuation.

In the second part of the Chapter, a planning method for closed kinematic chains is proposed. Using sample based planners, in this context has to face the fact that the probability to randomly select a point satisfying the closed loop constraint is zero. The proposed

ap-proach consists on (i) relaxing the closed loop constraints, obtaining a EM-like description of the system, and on (ii) a random sampling based sub-optimal planner. The union of the two allows to select equilibrium configuration starting from a random choice of the robot joint configurations. Later, an optimization method, based on the FGE, brings to find the values of all the other variables at the equi-librium. During the execution of the resulting path, the compensation of possible undesired contact forces, due to the constraint relaxation, can be left to the online controller.

1.3.2

Outline of part II:

Compliant Humanoid Robots

Chapter 5 shows how the tools developed in Chapter 2 can be used also for analyzing compliant humanoid robots in whole-body loco-manipulation tasks, that is considering also the possible presence of contact points in the internal parts of the robot. In such situation, despite the high degree of redundancy, the capabilities of the robot can be greatly compromised. After the discussion of the quasi-static equations of the system, the Fundamental Loco-Manipulation Equa-tion (FLME) is defined. The Algorithm GEROME-B, explained in Chapter 2, is here used to find the canonical form of the FLME. As for grasping, one of the main advantages of this formulation is to provide a global perspective on the problem. Relevant information on the system can be easily extract from the block composing the cFLMM, such as the controllable internal forces and the controllable displacements of the floating base and of the CoM.

Moreover, friction cone constraints, and minimum/maximum con-tact force limits are considered and used to build a convex formulation for the contact force optimization problem. The solution of the prob-lem bring to find the optimal controllable contact force distribution

in terms of distance (along a certain metric) from the contact limits. Numerical examples are presented, showing how the optimal contact force distribution problem can be used to find the maximum pushing force that can be applied on the environment, in a typical whole-body loco-manipulation task. Moreover, the best equilibrium configuration is found for a humanoid robot balancing on a slope varying friction condition on the feet. Finally, experimental tests performed with a COMAN robot demonstrates the effectiveness of the proposed meth-ods also in realistic scenario, bringing the robot to change its static equilibrium configuration, showing how results can vary depending by inclinations and friction conditions, even different for the two feet. Chapter 6 rests on results outcoming from Chapter 2 and Chap-ter 5. More precisely, the basic formulations both of the FGE and of the FLME are analyzed, discussing similarities and differences be-tween them. After that, a unified formulation is found introducing the Fundamental Whole-Body Interaction Equation (FWBE), able to describe the quasi-static behavior of both the system with a unique equation.

In Chapter 5 we showed that the tools developed in Chapter 2 for the analysis of underactuated hands can be used also for analyzing compliant humanoid robots. With the introduction of the FWBE, in-stead, we show that the two problems can also share some theoretical considerations and results. As an example, in the present Chapter some observations about the rank of the FWBE and the composition of the blocks of its canonical form are discussed with considerations valid for both the problems.

Part I

Chapter

2

Grasp Analysis Tools for

Synergistic Underactuated Robotic

Hands

The text of this Chapter is adapted from:

[A1] M. Gabiccini, E. Farnioli and A. Bicchi, Grasp and Manipu-lation Analysis for Synergistic Underactuated Hands Under General Loading Conditions. In: International Conference of Robotics and Automation ICRA 2012. Saint Paul, MN, USA; 2012. p. 2836 -2842.

[A2] M. Gabiccini, E. Farnioli and A. Bicchi, Grasp Analysis Tools for Synergistic Underactuated Robotic Hands. In: International Jour-nal of Robotic Research - IJRR - vol. 32, 11/2013, p. 1553 - 1576.

[A3] E. Farnioli, M. Gabiccini, A. Bicchi, Quasi-Static Analysis of Synergistically Underactuated Robotic Hands in Grasp and

Manipula-tion Tasks. In: Human and robot hands - Sensorimotor Synergies to Bridge the Gap between Neuroscience and Robotics, Springer series on Touch and Haptic Systems, In press.

2.1

Introduction

Neuroscience studies showed that humans mainly control their hands with coordinated motions, usually called synergies. In recent years, research on grasping and robotic hands posed the attention on this concept, trying to exploit its (conceptual) simplicity and its powerful, both from the design and the control point of view.

In this Chapter, tools for the quasi-static analysis of grasping will be presented, able to operate indifferently with fully actuated as well as synergistic underactuated systems, providing systematic methods to obtain relevant information on the local behavior of the physical system.

The methods, tools and results discussed in this Chapter will be the basis of all the rest of this thesis. Some of these results will be used, for example, to support the design of robotic hands, to develop planning methods for closed kinematic chains, as more extensively discussed in Chapter 1. Moreover, it is worth recalling also that the same approach will be used in the second part of this thesis in order to analyze a very different scenario such as humanoid robots interacting with the environment both with its end-effectors and with its internal links, in the so called whole-body loco-manipulation tasks.

More in detail, in the beginning of this Chapter, after the presen-tation of the human-inspired concept of soft synergy, the analytical model of the grasping problem with underactuated hands is presented in Section 2.3, based on the screw theory [32], [33], [34], [35]. Since also the derivative terms of the Jacobian matrix are taken into account, configurations with contact force preload can be rigorously analyzed

within the proposed framework. Moreover, an object-centric formu-lation is used, obtaining, as a consequence, a constant grasp matrix and some simplifications in describing the rigid displacements of the grasped object.

As has been mentioned previously, the whole set of quasi-static equations gives life to the FGE, from which it is possible to extract a large number of information on the behavior of the physical system, at least in the neighborhood of the initial equilibrium configuration. A first approach to elaborate the FGE is presented in Section 2.4. Here, the Gauss row operation method is extended to operate on block par-titioned matrices. The resulting algorithm applied to the FGM brings to obtain a new formulation, called canonical form of the Fundamen-tal Grasp Equation (cFGE), and its coefficient matrix, the canonical form of the Fundamental Grasp Matrix (cFGM). From the cFGE, the relationships between the controls and the consequent hand/ob-ject configurations and forces can be found, both in numerical and in symbolic form. Moreover, as discussed in Section 2.5, relevant proper-ties of the grasp, as a matrix spanning the controllable internal forces, a matrix spanning the controllable object displacements and the grasp compliance, can be simply obtained extracting proper blocks from the cFGM.

In Sections 2.6 and 2.7, some manipulation tasks are defined in terms of nullity or non-nullity of the system variables. As examples, we mention the pure squeeze of the grasped object, where a contact force variation is admitted without changing the object configuration, and the kinematic grasp displacements, in which the object is moved without affecting the initial squeezing forces.

In Sec. 2.7 we show how a decomposition method, based on the research of the reduced row echelon form (RREF), initially presented in [36], can be applied to proper portions of the solution space of the system, that is of the nullspace of the FGM, in order to evaluate the

feasibility of such predefined tasks.

The idea to distinguish different kinds of solutions is expanded in Section 2.8. A sieve of Eratosthenes approach is used to discover which combinations of nullity or non-nullity of the system variables are allowed by the FGE, until a complete classification of the solu-tion space is obtained and graphically synthesized in the taxonomic labyrinth. In Section 2.9 some words are spent to describe how the methods proposed in the Chapter can be applied to the study of robotic hands characterized by other types of (under-)actuation. This fact will be better investigated in Chapter 3, where the possibility to obtain a soft synergy like controllability using the adaptive synergy underactuation mechanism [37] will be studied. Moreover, it will be described how these concepts were used to develop a robotic hand prototype [A4], [A5] able to achieve both the results to get human-inspired movement and an easier mechanical implementation.

To conclude the Chapter, in Section 2.10, numerical examples are presented for tip and power grasps. Both cases are firstly studied as if the hand were completely actuated, finding out its manipula-tion capabilities. Then, the synergistic underactuamanipula-tion is introduced verifying which possibilities are lost and which other still persist.

2.2

Human-Inspired Underactuation

The design of robotic hands was directed for long time to maximize the dexterity and the manipulation capabilities. This goal was sought in-creasing complexity and number of degrees of freedom. Following this trend, remarkable examples of robotic hand designs were proposed, as for example the UTAH/MIT hand [38], the Robonaut Hand [39], the Shadow hand [40], the DLR hand II [41], and the DLR hand arm system [42]. One of the main issues of this approach is that a large number of degrees of freedom implies many actuators, with a

conse-quent growth of size, weight and cost. With such kind of devices, an-other disadvantage is the difficulty in control. Finding a joint control law, able to perform a given task, may become a difficult job, mainly due to the complex kinematic structures of the hands. Starting from these observations, different kinds of underactuation strategies were introduced and studied [43], [44]. In this direction, a recently consid-ered approach is to take inspiration from the human hand, not just from a biomechanic point of view, but also for the control aspects.

Despite significant differences in the definitions and in the require-ments of the investigated tasks, many neuroscience studies such as [45], [46], [47], [48], [49] [50], [51] and [52], to mention only a few, share a main observation: simultaneous motion of multiple digits oc-curs in a consistent fashion, even when the task may require a fairly high degree of movement individuation such as grasping a small ob-ject or typing.

One of the main quantitative results of these studies is that a large variety of common grasps that humans can do is well described by just five synergies, being the first two important enough to explain 80%of the variance in grasp postures. This suggests the possibility to change the basis of the description, from the joint space to the human-inspired postural synergy space. In [53] this idea was implemented via software to control the hand for grasp planning, interrupting the rigid coordination of a finger when it arrives in contact with the object to grasp. A second way, proposed in [54], follows the road of an hard-ware implementation, where two kinds of synergies can be achieved by changing the set of actuation pulleys. In this case, when a first contact with the object achieved, either stable or not, the hand has to stop its motion.

Differently from the previous one, the soft synergy approach, firstly discussed in [31] and in [55], avoids the problems due to a strict corre-lation between joint motions. In this case, the synergistic movement

is imposed to a virtual hand which attracts the real one via a gen-eralized spring. The influence of synergies in motion and forces is investigated in [56]. The contact force optimization problem was in-vestigated in [55], taking into account the limitation imposed in the force controllability by the underactuation. In next Sections of this Chapter the grasping problem in presence of soft synergy underac-tuation will be extensively discussed, together with the quasi-static analysis tools developed to systematically investigate the capabilities of such systems.

2.3

Quasi-Static Model of the System

A basic assumption in this work is that all bodies are modeled as rigid. Thanks to this, it is possible to use the wide variety of mathematical tools developed to describe rigid body motions. However, the presence of compliance in the system is allowed in a lumped way. For example, as we will further discuss later, the softness of the contact fingertip and/or the grasped object can be described by introducing contact virtual springs.

It is worth observing that, as usually done in literature, we sepa-rate the investigation of the system capabilities, e.g. the contact forces on the object actually controllable by the hand, to the restriction im-posed by the friction limits, assuming that a subsequent classical force optimization procedure can lead to a feasible solution.

With reference to Figure 2.1, we model a hand as a collection of serial robots manipulating an object. An inertial frame {A} = {Oa; xa, ya, za} is attached to the palm. On the ithof p contact points,

we place a frame C_ih = n

O_ch

i; xchi, ychi, zchi

o

attached to the finger link, and a frame {Co

i}=

 Oco

i; xcoi, ycoi, zcoi

attached to the object. A reference frame {B} = {Ob; xb, yb, zb} is fixed on the grasped object.

qr fc τ we σr q {A} {B}

Figure 2.1: Compliant grasp by an underactuated robotic hand.

Later in the text we will refer to it also as the body frame or as the object frame. With the symbols ξz

xy we indicate the twist of the frame

{Y } w.r.t. {X}, as described by an observer attached to {Z}. De-noting with gtz∈SE(3) the posture of frame {Z} with respect to {T },

according to [33], we define the adjoint matrix, Adg ∈ R6×6: se(3)→

se(3), as an operator able to transform twists according to the law ξ_xyt =Adgtzξ

z

xy. (2.1)

Indicating with wy

xthe wrench that the frame {X} exerts on the frame

{Y }, expressed in {Y } components, we introduce the co-adjoint ma-trix operator, Ad−T_{g ∈ R}6×6: se∗(3)_{→ se}∗(3), transforming wrenches according to the law

w_xt =Ad−T_gty wy_x. (2.2) For reader convenience, the explicit form of the homogeneous and

adjoint operators is given in Appendix A. Other definitions and the notation used in the text are summarized in Table 2.1.

Notation Definition

δν variation of variable ν ¯

ν value of variable ν in reference equilibrium configuration ]ν dimensions of vector ν

{A} palm (inertial) frame 

C_ih ith _{contact frame attached to the hand} {Co

i} ith contact frame attached to the object {B} object frame

q∈ R]q _{joint variables}

qr∈ R]q joint reference variables τ _{∈ R}]q joint torques

σ_{∈ R}]σ synergy variables

σr∈ R]σ synergy reference variables η_{∈ R}]σ synergy generalized forces

c number of hand/object interaction constraints u∈ R6 _{position and orientation of the object frame} ξzxy∈ R6 in the motion of the frame {Y } relative to {X},

with components expressed in {Z} fcoi

ch

i ∈ R

ci contact force/torque exerted from_Ch

i

to {Co i} with components and moments relative to the frame {Co

i}

wy

x∈ R6 wrench exerted by {X} on {Y }, with components and moments relative to {Y }

a_J

∈ Rc×]q _{hand Jacobian matrix in inertial frame}

co

J _{∈ R}c×]q hand Jacobian matrix in object contact frames S _{∈ R}]q×]σ synergy matrix

b_{G∈ R}6×c _{grasp matrix in body frame}

2.3.1

Equilibrium Equation of the Object

e ∈ R6 be the external wrench acting on the object with

compo-nents expressed in {B}. Similarly, we indicate with wco i ch

i the wrench

that the hand exerts on the ith_{contact point on the object, with}

com-ponents in {Co

i}. The equilibrium of the frame {B} requires that the

sum of all force/moment contributions, with components in {B}, is null. This condition can be written as

wb_e+ p X i=1 Ad−T gbco_i w co i ch i = 0. (2.3)

The hand/object interaction can be described as a six-dimensional constraint for the position and orientation of the local contact frame. Usually, however, depending on the nature of the contact type, the constrained directions are less than six. In this way, we assume to describe the contact interaction with a vector fco

i ch

i ∈ R

ci, with c i ≤ 6,

that takes into account only the directions constrained. More details will be provided in Section 2.3.5. For each contact point we introduce a matrix Bi ∈ R6×ci able to map the local interaction in a complete

wrench as follows wcoi ch i = Bif co i ch i . (2.4) Substituting (2.4) in (2.3), we obtain wb_e+ p X i=1 Ad−T gbco_i Bif co i ch i = 0. (2.5) Defining c =Pp

i=1ci as the total number of contact constrain acting

on the object, we define the grasp matrix in body frame, b_{G ∈ R}6×c_,

as b_{G =}h_Ad−T gbco₁ B1 · · · Ad −T gbco_i Bi · · · Ad −T gbco_pBp i . (2.6)

Equation (2.6) allows us to express the object equilibrium equation in the form w_eb+bGf_ccho = 0, (2.7) where fco ch= [f co 1T ch 1 , . . . , fcopT ch p ]

T_{∈ R}c _{is a vector collecting all the actions}

performed by the hand on the object. It is important to underline that the grasp matrix in body frame is a constant matrix if the object contact points do not change. In consequence of this, to find a law relating small perturbations of the variables involved, we can calculate the differential of (2.7). It follows that the perturbed equilibrium equation can be written as

δw_eb+bGδf_ccho = 0. (2.8)

2.3.2

Congruence Equation of the Object

The twist of the ithcontact frame {Co

i} on the object can be described

as a function of the twist of the object frame {B}. To do this we use the adjoint matrix, presented in (2.1), as follows

ξcoi

ab =Adgco_ibξ b

ab. (2.9)

As briefly discussed before, usually the contact constraints do not involve all the displacement directions. As explained in [30], the ma-trix BT

i is able to select the terms of velocity violating the contact

constraints, as well as the matrix Bi in (2.4) can expand the

force/mo-ment vector in a complete wrench. Introducing the vector vco i

ab ∈ Rci,

containing only the terms of the ith _{velocity violating constraints, the}

equation relating the body frame twist and the velocity of the ith

contact point frame can be expressed as vcoi ab = BiTξ co i ab = BiTAdgco_ibξ b ab. (2.10)

Defining vco ab = h vco1 ab T · · · vcoi ab T · · · vcop ab TiT ∈ Rc_{, using (2.9) and (2.10), we} can write v_abco =hB₁T Adgco_1b T · · · B_iT Adgco_ib T · · · B_pT Adgcopb TiT ξ_abb . (2.11) By a direct comparison between (2.11) and the definition of the grasp matrix in (2.6), immediately follows that

v_abco =bGTξ_abb . (2.12) Remembering that the rotational part of the term ξb

abdt does not

represent a variation of coordinates, we have to introduce a vector u_∈R6 able to parametrize SE(3). A possible approach is to choose a parametrization vector composed by the same translational part of ξb

ab

and Euler’s angles in the rotational part, as described in [57]. A con-venient approach is to introduce a virtual kinematic chain, describing the object configuration with respect to the absolute frame. In this case the object Jacobian matrix, that is the Jacobian of the virtual chain, can be used to parametrize the object twist, as follows

ξ_abb = Jo(u) ˙u. (2.13)

Defining the variation of the contact frame of the object as δCco ab =

vc_abodt, we can substitute (2.13) in (2.12). Introducing the matrix

b_GT

t(u) = bGTJo(u), we can write the congruence equation of the

object as

δC_accoo =bGT_t(u)δu. (2.14)

2.3.3

Congruence Equation of the Hand

Letting the vector ξa ach

i ∈ R

6 _{be the twist of the frame} _Ch i

with respect to {A}, with components expressed in {A}, as described in [33]

and summarized in Appendix B, we introduce the spatial Jacobian matrixa_J

i(qi)∈ R6×]qi, where ]qi is the number of joints that precedes

the ith _{hand contact point in its own serial chain. This allows us to}

write the relationship

ξ_acah

i =

a_J

i(qi) ˙qi. (2.15)

An object-centric description is obtained mapping (2.15) from the in-ertial frame {A} to object frame {Co

i}, thus using the adjoint operator

as follows ξcoi ach i =Ad_gco ia(u)ξ a ach i =Adgcoia(u) a_J i(qi) ˙qi. (2.16)

The selection matrix BT

i can be used, as in (2.10), to maintain the

terms of velocity in the directions violating contact constraints. This results in vcoi ach i = B T i Adgco_ia(u) a_J i(qi) ˙qi =c o i_J i(qi, u) ˙qi. (2.17)

Similarly to what previously done, defining v_accoh = h vco1 ach 1 T · · · vcoi ach i T · · · vcop ach p TiT ∈ Rc _(2.18)

and collecting all the joint parameters in the vector q ∈ R]q_{, from}

(2.17) we can write

v_accoh =c o

J (q, u) ˙q, (2.19)

where co

J (q, u) is the hand Jacobian referred to the object contact frames. Multiplying (2.19) by dt, we find the congruence equation of the hand as

δC_accoh =c o

J (q, u)δq. (2.20)

2.3.4

Equilibrium Equation of the Hand

From the kineto-static duality, coherently with the expression of the hand Jacobian, the map from the forces exerted by the hand fco

the joint torques τ ∈ R]q _{is given by}

τ =coJT(q, u)f_ccho. (2.21)

Small perturbations of the system can be described by differentiation of (2.21), obtaining δτ = ∂ co JT(q, u)f_ccho ∂q δq + ∂ coJT(q, u)f_ccho ∂u δu + + ∂ co JT(q, u)f_ccho ∂f δf co ch. (2.22)

Defining the matrices Q = ∂ co_JT_(q,u)fco ch ∂q ∈ R]q×]q, U = ∂co_JT_(q,u)fco ch ∂u ∈ R]q×6, (2.23)

the equation describing the perturbed equilibrium of the hand can be expressed as

δτ = ¯Qδq + ¯U δu +coJ¯Tδf_ccho. (2.24)

Evaluating (2.24) in an equilibrium configuration, the terms Q and U become negligible if the initial contact forces are small or null. In Ap-pendix C we present a method to compute the derivatives in (2.23), based only on the knowledge of the elements composing (2.17), that is the spatial Jacobian and adjoint transformations, in an algorithmic way. From a practical point of view, this implies that neither sym-bolic calculations nor finite differences computation are required to compute the derivatives in (2.23).

2.3.5

Hand/Object Interaction Model

The contact between the hand and the object can be interpreted as a motion constraint. Typically, depending on the nature of materials

involved, not all the directions of motions are limited. As an example, for the case of contact point without friction, only the direction normal to the contact surface is forbidden. This contact type is equivalent to having only one constraint, thus the dimension of the vector fco i co i

is ci = 1. Similarly, the contact point with friction, or hard finger,

allows the presence of three components of force, on the contact, but no moment, ci = 3. As a last example we cite the soft finger case.

With respect to the hard finger, it adds the possibility to transmit a moment around the normal vector to the contact surface, ci = 4.

In general, we are not justified to assume that the total number of motion constraints imposed by the interaction with the hand is equal to the degrees of freedom of the grasped object (six in 3D case, three in a planar case). On the contrary, we usually have more con-straints that degrees of freedom, and this makes the problem hyper-static, a.k.a. statically indeterminate. In these cases, the equilibrium equations are not sufficient to univocally determine the contact forces. To tackle this problem, we introduce a contact force model which is linear in the interference between the hand and the object. More specifically, for the ith _{contact point we introduce a contact stiffness}

matrix K_{ci ∈ R}ci×ci containing the characteristics of the virtual

con-tact spring. Accordingly, the ith _{force variation that the hand exerts}

on the object can be expressed as δfcoi ch i = Kci(δC co i ach i − δC co i aco i). (2.25)

Introducing Kc = blkdiag(Kc1, . . . , Kcp) ∈ Rc×c, as a block diagonal

matrix, where the ith _{block element is K}

ci, the constitutive equation

of the contact forces becomes

δf_ccho = Kc(δCc o

ach− δCc o

2.3.6

Joint Actuation Model

For the ith_{joint, we introduce an elastic model, which relates the real}

configuration with a reference one, qri∈ R, by a torsional spring with

constant stiffness kqi∈ R, by the following

τi = kqi(qri− qi). (2.27)

Defining Kq =diag(kqi)∈ R

]q×]q _{as the matrix collecting all the joint}

stiffness, and introducing qr ∈ R]q as the vector collecting the joint

reference variables, by differentiation we can write the quasi-static actuation law as

δτ = Kq(δqr− δq). (2.28)

2.3.7

Underactuation of the Hand

The equations presented up to here allows the description of a hand with elastic joints, but with an equal number of degrees of actuation and freedom. However, it is possible to extend the problem to include underactuated systems as well. To this end, we impose that the joint reference positions qr evolve on a manifold S ⊆ R]q as a function of

some postural synergy values σ ∈ R]σ_{, where ]σ ≤ ]q. Thus, in general}

we can write

qr = f (σ), (2.29)

where f : R]σ_{→ S ⊆ R}]q _{is the synergy function. Calculating the}

dif-ferential of (2.29), and defining the synergy matrix as S(σ) = ∂f (σ)

∂σ ∈

R]q×]σ, the joint reference displacements are described as follows

Again, by virtue of the kineto-static duality, we can find that the vector η ∈ R]σ _{of the generalized forces at the synergy level is related}

to the joint torques by the relationship

η = ST(σ)τ. (2.31)

Differentiating (2.31) and introducing the matrix Σ(σ, τ) = ∂ST_(σ)τ

∂σ ∈

R]σ×]σ, for the actuation forces we obtain

δη = ¯STδτ + ¯Σδσ. (2.32)

For the sake of generality, we can introduce an elastic model even for the synergistic underactuation, similarly to what done before for the joints. Defining the synergy stiffness as the matrix Kσ =diag(kσi)∈ R]σ×]σ, and introducing the vector of synergy references σr ∈ R]σ, the

actuation law appears as

δη = Kσ(δσr− δσ). (2.33)

2.3.8

The Fundamental Grasp Matrix (FGM)

The set of equations composed collecting (2.8), (2.14), (2.20), (2.24), (2.26), (2.28), (2.30), (2.32) and (2.33) is able to describe a linear approximation of all the possible system displacements around an equilibrium configuration. The result is the linear and homogeneous equation, called Fundamental Grasp Equation (FGE), that can be written in compact form as

Φ?δϕ = 0, (2.34)

where the coefficient matrix Φ? _{∈ R}rΦ×cΦ is called Fundamental Grasp

variables of the system is the augmented configuration. More in detail, the parts composing the FGE in (2.34) appear as

Φ?=                       I]f 0 0 0 −Kc Kc 0 0 0 0 0 −co_¯ JT I]τ 0 − ¯U 0 0 − ¯Q 0 0 0 0 0 _{− ¯}ST I]η 0 0 0 0 0 −¯Σ 0 0 b_{G 0 0 0 0 0 0 0 0 I} ]w 0 0 0 0 0 I]Ch 0 −c o_¯ J 0 0 0 0 0 0 0 ₋bG¯T_t 0 I_]Co 0 0 0 0 0 0 I_]τ 0 0 0 0 K_{q −Kq} 0 0 0 0 0 0 0 0 0 0 I_{]qr − ¯}S 0 0 0 0 I_]σ 0 0 0 0 0 K_σ 0 _−Kσ                       , δϕ =h δf_cchoT δτT δηT δuT δCc oT ach δCc oT aco δqT δqT_r δσT δw_ebT δσ_rT iT ,

where we indicated with ¯ν the value of the variable ν in the starting equilibrium configuration, and with the symbol I]ν an identity matrix

of dimension ]ν.

It is trivial to note that the constraint identified by (2.34) does not change its physical implications using a different order of both the equations and the variables. For reasons that will be clarified in Section 2.4.2.4, the chosen order of the equations and of the aug-mented configuration components derives from the specification to have square blocks on the main diagonal. However, this character-istic can always be obtained simply applying suitable permutation matrices to any order of equations and variables.

2.4

Controllable System Perturbations

It is easy to find that the dimensions rΦ and cΦ of the FGM in (2.34)

correspond to

r_Φ = ]w + 3]τ + 3]f + 2]σ,

c_Φ = 2]w + 3]τ + 3]f + 3]σ. (2.35) In the very general cases of interest, it results rank(Φ) = rΦ, and

we will assume it in the rest of the text. Exceptions are analytically possible but they refer to pathological situations of poor practical relevance.

All the solutions of (2.34) can be written by a basis for the nullspace of the FGM. We indicate this with the symbol Γ ∈ Rrγ×cγ. Taking

into account (2.35), it immediately follows that rγ = cΦ,

cγ = cΦ− rΦ = ]w + ]σ.

(2.36) This implies that we can find an unique solution of (2.34) when it is known a number of independent variables, or inputs for the system, equal to cγ = ]w + ]σ.

In continuity with the grasp analysis literature, we choose to con-sider the external wrench variation δwb

e as known or measurable.

Moreover, we consider the synergy references as position-controlled. This corresponds to the practical case of an actuation by servo motors to control the synergy displacements δσr. Other choices are possible,

maintaining constant cγ, for example by substituting the external

wrench δwb

e with the object displacement δu, or the reference synergy

positions δσr with synergy force actuation δη. The results of our

anal-ysis can be easily adapted to the above mentioned situations as well. Next Sections will show two different methods to obtain the