Object Re-grasping with Dexterous Robotic Hands

Ana Carolina Grilo da Silva Januário

Thesis to obtain the Master of Science Degree in

Electric and Computer Engineering

Supervisor(s): Prof. Alexandre José Malheiro Bernardino

Examination Committee

Chairperson: Prof. João Fernando Cardoso Silva SequeiraSupervisor: Prof. Alexandre José Malheiro Bernardino

Member of the Committee: Prof. Rodrigo Martins de Matos Ventura

December 2015

ii

Resumo

Esta tese estuda a manipulacao em maos roboticas destras, em particular, a habilidade de alterar o

tipo de agarre de um objecto enquanto este esta seguro na mesma mao. Esta habilidade e bastante

importante visto que, para o desempenho de tarefas com o objecto, e importante alterar a forma como

o objecto e agarrado pela mao. Por exemplo, quando agarramos uma caneta com o intuito de escrever,

normalmente temos que manipula-la, alterando para a pose correcta de agarre para escrever. E um

problema desafiante visto que mover os dedos, pode alterar algumas propriedades de estabilidade.

Tratamos este problema atraves da abordagem de reposicionamento dos dedos no objecto. Dado

um agarre inicial, movem-se os dedos sequencialmente, de um ponto na superfıcie do objecto para

outro, alcancando a posicao final desejada. E um problema desafiante na medida em que, ao remover

um dedo de um agarre estavel, podemos alterar alguma propriedade de estabilidade fazendo o ob-

jecto cair. Desse modo, desenvolvemos um algoritmo de planeamento que escolhe a sequencia de

reposicionamento dos dedos mantendo a estabilidade durante todos os passos de reagarre.

A implementacao deste algoritmo e feita utilizando as ferramentas ROS-Gazebo-MoveIt. Este con-

junto engloba um ambiente de simulacao robotica, deteccao de (auto-)colisao e planeamento de movi-

mentos. Assim, os algoritmos desenvolvidos podem ser adaptados a outros robos.

Sao apresentados resultados em simulacao para a mao do robo iCub. Assumimos que um metodo

apropriado de alcance ao objecto coloca a mao robotica perto do objecto com uma pose conveniente

para o agarre. Assim, fazemos o planeamento de movimentos, evitando colisoes, para que os dedos

completem o primeiro agarre. De seguida, adquirimos e processamos a informacao relativa aos contac-

tos atraves de sensores tacteis para entao computar as propriedades de estabilidade do agarre (teste

de fecho de forca). Depois, comecamos por testar que contactos podem ser removidos sem perder es-

tabilidade. Finalmente, procedemos ao reposicionamento dos dedos, mantendo um fecho de forca, ate

que o agarre final seja alcancado. No final, avaliamos a nossa abordagem em termos de propriedades

de estabilidade ao longo da sequencia de reagarre, e da robustez relativa a pequenas variacoes na

forma dos objectos.

Palavras-chave: reagarre, reposicionamento de dedos, fecho de forca, mao destra, ROS,

Gazebo, deteccao de colisoes

iii

iv

Abstract

This thesis addresses in-hand manipulation for dexterous robotic hands, in particular with the ability of

changing the grasp type on an object while holding it. This is an important skill, since for the execution

of many tasks it is important to change the way the object is grasped in the hand. For instance, when

we grasp a pen to write, typically we have to manipulate it in the hand to place it in the right pose for

writing. This is a very challenging problem because moving the fingers on the object may change its

stability properties.

We address this problem via a finger gaiting approach. Given an initial grasp, move the fingers in

sequence, from one location to the other in the surface of the object, to achieve a desired final position.

This is a challenging problem because removing one finger from the surface of the object may change

its stability properties and make the object fall. Therefore, we develop a planning algorithm that chooses

the best sequence of finger gaits to maintain stability during the re-grasping steps.

We have implemented the algorithm using the pipeline ROS-Gazebo-MoveIt. This pipeline already

includes a generic robot simulation environment, (self-)collision detection, and motion planning. Thus,

the developed algorithms could be adapted to other robots.

Results are presented in simulation for the iCub robot hand. We assume that an appropriate reaching

method has put the robot hand close to an object in a convenient pose to grasp. Then, we do the motion

planning, with collision avoidance, for the fingers to accomplish the first grasp. Next, we acquire and

process the contact information from the tactile sensors so we can compute the stability properties of

the grasp (force closure test). Then, we start testing which contacts can be removed without loosing

stability. Finally, we proceed to the fingers repositioning, keeping a force closure grasp, until the final

grasp is reached. We evaluate our approach in terms of the stability properties along the re-grasping

sequence and robustness to object small shape variations.

Keywords: re-grasp, finger gaiting, force closure, dexterous hand, ROS, Gazebo, collision

avoidance.

v

vi

Contents

Resumo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii

Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . v

List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix

List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xi

Glossary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1 Introduction 1

1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Topic Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.3 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.4 Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.5 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.6 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Background 5

2.1 Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.1.1 Grasp Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.1.2 Force Closure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.1.3 Collision Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.1.4 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

2.2 State of The Art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2.2.1 Grasping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2.2.2 Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3 Outline of the Proposed Algorithm 31

3.1 Finger Gaiting Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

4 Implementation 35

4.1 ROS Development Platform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

4.1.1 Brief Explanation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

4.1.2 Software Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.2 Gazebo Simulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

vii

4.2.1 Brief Explanation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.2.2 The best simulator for the iCub humanoid robot . . . . . . . . . . . . . . . . . . . . 38

4.2.3 Gazebo and ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.3 Hand Model for Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.4 Contact Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.5 Force Closure Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.6 Motion Planning and MoveIt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

4.6.1 Reaching the Object . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

5 Results 49

5.1 Problem Description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

5.2 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

5.3 Reaching the First Grasp Pose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.4 In-hand Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.5 Force Closure Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

5.6 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

6 Conclusions 55

6.1 Achievements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

Bibliography 57

viii

List of Tables

2.1 Components transmitted and not transmitted through the contact in each contact model. . 7

2.2 Table with the most common contact models. Adapted from [4]. . . . . . . . . . . . . . . . 8

4.1 Most important features for a simulator. Table from [38]. . . . . . . . . . . . . . . . . . . . 39

4.2 Most important criteria for choosing a simulator. Table from [38]. . . . . . . . . . . . . . . 39

4.3 Set of time samples from the topic we acquire contact information. Notice the inconsis-

tency of information during time without any motion of the fingers during the acquisition. . 45

ix

x

List of Figures

1.1 Hand model on Gazebo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2.1 Two examples of friction cone. The example on the left is a friction cone with four itera-

tions. The example on the right has six iterations. Image source [4]. . . . . . . . . . . . . 9

2.2 Example of a grasp with two fingers of a box. Image source [4]. . . . . . . . . . . . . . . . 9

2.3 A set of five wrenches represented in the wrench space (abscissa- force; ordinate-torque).

Image source [1]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.4 a)AABB , b)OBB. Image source [10] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.5 Example of kIOS applied in yellow object. The curves in blue means the 3 spheres that

origins the bounding volume(in red). Image source [12]. . . . . . . . . . . . . . . . . . . . 13

2.6 a)Dois objectos em colisao. b) Polıtopo e pontos resultantes da Diferenca de Minkowski. 14

2.7 a)Dois objectos sem colisao. b) Polıtopo e pontos resultantes da Diferenca de Minkowski. 15

2.8 Example of EPA with 2 iterations: a)First edge closest to origin selected, b)First edge

closest to origin expanded. Second edge closest to origin selected, c)Second edge clos-

est to origin expanded. A Edge of A − B polytope is the closest to the origin. Expansion

terminated. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.9 a)Overlapping objects seen from xx axis, b)Non-overlapping objects seen from yy axis,

c)Non-overlapping objects seen from zz axis, d)Set of pairs of objects: First contains pairs

which overlapping on xx axis; Second contains pairs which overlapping on xx and yy axis;

Third contains pairs which overlapping on xx, yy and zz axis. . . . . . . . . . . . . . . . . 17

2.10 a)Overlapping objects seen from xx axis, b)Overlapping objects seen from yy axis, c)Overlapping

objects seen from zz axis, d)Set of pairs of objects: First contains pairs which overlapping

on xx axis; Second contains pairs which overlapping on xx and yy axis; Third contains

pairs which overlapping on xx, yy and zz axis. . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.11 Example of two SV and their collision. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

2.12 a)BVH result of an articulated object. b) BVH based on AABB(see 2.1.3.1). c) BVH based

on OBB(see 2.1.3.1). Dashed borders represent BV boundary. Continuous lines mean

original object boundaries. Adapted image from [19] . . . . . . . . . . . . . . . . . . . . . 20

2.13 a)BVH of object A ; b)BVH of object B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

2.14 Resulting BVTT of objects in figure 2.13. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

2.15 Example of KPIECE discretization with three levels. Image from [22] . . . . . . . . . . . . 23

xi

2.16 Example of RRT method application without final state specification. The image repre-

sents the evolution of the tree during the method from the left to the right. Image from

[23]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

2.17 Example of PRM method application in an environment with some objects(in gray). The

graph is composed with gray edges and orange vertex and the solution is the set of orange

edges and vertices.Image from [25] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2.18 8 different kinds of grasps already implemented. Image from [27]. . . . . . . . . . . . . . . 26

2.19 Example of an object reorientation by finger gaiting. Image from [33]. . . . . . . . . . . . . 28

2.20 Example of a sphere manipulated with finger substitution. Image from [31]. . . . . . . . . 28

2.21 Example picking and dropping an object. Image from [34]. . . . . . . . . . . . . . . . . . . 29

3.1 Flowchart illustrating the proposed algorithm (see text). . . . . . . . . . . . . . . . . . . . 33

4.1 Simple example of topics and nodes interaction (publications and subscriptions). . . . . . 36

4.2 Example of a ROS tf tree. Each edge represents a transform from the parent node to the

child node. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.3 ROS structure of this algorithm (see text). . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.4 Gazebo an ROS architecture scheme (see text). Image from (http : //gazebosim.org/tutorials/?tut =

ros control). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.5 Minimum and maximum angles of the model hand joints (see text). . . . . . . . . . . . . . 41

4.6 Index Finger touching an object (see text). . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.7 Metric Results using different levels of discretization. . . . . . . . . . . . . . . . . . . . . . 45

5.1 Objects used in this algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

5.2 The Version of Parallel extension grasp used in this work. . . . . . . . . . . . . . . . . . . 50

5.3 Index Ring to Middle grasp (IR2M). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.4 a) The hand and the object in the simulation environment. b) First grasp pose. . . . . . . 50

5.5 4 possible re-grasp sequences tested with the algorithm. . . . . . . . . . . . . . . . . . . 51

5.6 4 possible re-grasp sequences tested with the algorithm. . . . . . . . . . . . . . . . . . . 52

5.7 Metric Results using mean calculation, in blue, and using all contact points, in orange. . . 53

xii

Chapter 1

Introduction

The contents of this chapter will be the motivation to develop the work. Next, it will be presented the

state of the art required, the objectives/goals to achieve and finally, the thesis outline.

1.1 Motivation

Robotics is an expanding area of knowledge, specially when it is about grasping with dexterous hands

because they are such a complex part of the human body. When imagining our future, we would like

robots to do somethings that we, humans, do not appreciate so much, like housework. The most objects

we interact on a daily basis were built for humans so anthropomorphic hands are the natural manipulation

devices for such objects. This kind of hands has many degrees of freedom, which gives them more ability

than simple grippers. This complexity also makes the control of these devices harder.

For robot automation turn reality, the robot must be able to pick an object and change the pose of

the fingers to grasp it in a way that can use the object in some task. The process associated with that

change between grasps is the re-grasping. In some manipulation cases, it is considered the re-grasp

with rolling and sliding. Rolling is the phenomenon when the contact is not removed, but the surface of

contact change by rolling the fingers on the object surface. The contact area of the finger changes as

the zone of the object where the contact is applied. The slide phenomenon happens when the friction

is smooth and the fingers can slip through the object surface. There is another technique named finger

gaiting that consists in lift the finger, removing the current contact and reposition the finger in a new

pose on the object surface. We will approach finger gaiting technique mainly because with finger gaiting

we can reach the same poses reached by finger rolling and finger sliding. We will not consider rolling

neither sliding phenomena.

The main problem of this work is to give the necessary information so the in-hand manipulation

becomes possible to accomplish, always ensuring that, during the finger gaiting, the object do not fall.

We need to move the fingers from one position to another one avoiding undesired collision situations.

So we do motion planning, with collision avoidance and only touch the object on the desired pose for the

final grasp.

1

The model that will be used is based on the iCub robot hand (http://www.icub.org/). However, the

model is adapted to have one controller for each joint, which makes it different from the iCub robotic

hand since the real model has some joints coupled sharing the same controller. The hand model has

nineteen joints: each finger has one joint for abduction (with the exception of middle finger), one for

proximal, other for intermediate and another for distal. The figure 1.1 has the hand model used for this

work described.

Figure 1.1: Hand model on Gazebo.

1.2 Topic Overview

As people grab objects, anthropomorphic robot hands should grab objects in the same way. In an almost

automatic way, we pick an object with enough stability for the object do not drop if some external force is

applied. This stability is verified using a test named Force Closure Test([1]). This technique is essential

in this work since we need grasps to be stable so the object is not thrown in some direction due to some

uncompensated force that was applied.

A grasp is defined as a set of contact poses between the fingers and the object surface, the corre-

spondent applied forces and material properties, like friction coefficients. The problem to solve is, given

a certain grasp, to be able to identify unnecessary fingers to have a stable grasp, and be able to move

those fingers to other positions to reach some other grasp previously selected.

1.3 Objectives

The main objective of this work is to develop a technique for in-hand object re-grasping through finger

gaiting. This technique basis is to remove the current contact and then, reposition the finger on a different

part of the object. First, we need to identify the expendable fingers from grasps, so it becomes possible

to move these fingers to the final positions without destabilising the grasp.

We propose to combine techniques already developed in similar areas to accomplish the re-grasping

method such as some present in ROS (Robot Operating System) like the package MoveIt which con-

tains some libraries of motion planning such as OMPL (Open Motion Planning Library) [2] and collision

2

detection, FCL(Flexible Collision Library)[3]. These libraries will be explained in chapter 2. Another

technique to be studied is the force closure test also studied in [1] and [4].

More specifically, the manipulation method we present is holding the object and change some fingers

to other poses to reach the final grasp. That final grasp has the purpose of the robot to do a task by

holding the object with that configuration.

1.4 Assumptions

We have to take into account object’s characteristics required for this problem. Objects need to be:

• Rigid, so they can be simulated using standard tools;

• Light, so the robotic hand is strong enough to compensate any external forces such gravity and

inertia;

• High friction coefficient, so they do not slip over during the manipulation.

We assume that the robotic hand controller is able to compensate external disturbances and keep

the object static in the hand. The control for rejecting disturbances will not be developed in this work.

1.5 Contributions

With this work, we aim to contribute developing a safe framework for finger gaiting with an anthropomor-

phic robotic hand in 3 dimensions. As previously referred, we will use a hand model similar to the iCub

robotic hand enhancing the re-grasping applications of the robot.

We will use collision detection features from MoveIt to avoid and generate collision while removing

the current contact and creating the desired contact, respectively. The contact model to be used and

implemented is the soft contact model since it is a more realistic model to represent contacts.

To develop the method, we will use the tools available in ROS, Robot Operating System, and as

simulation tool, we will use Gazebo Simulator.

1.6 Thesis Outline

This thesis is composed of five chapters, organized as follow. In chapter 2 we present the background

knowledge we will use to get the solution of this problem and make a brief overview of the state-of-the-art

in grasping and manipulation.

In chapter 3 we introduce the proposed algorithm to achieve the mentioned goals.

In chapter 4 it will be introduced the software used such ROS, Gazebo Simulator, the model hand

used and how to compute contacts between the fingers and the object. We also describe how to use the

force closure detection and how to to motion planning for each finger. Finally, we present the structure

of the proposed algorithm using the previous explained techniques.

3

In chapter 5 there are presented the results of applying the methods by describing the inputs, outputs

and sequence of tasks required to show the algorithm result.

Finally, chapter 6 concludes our work and discuss some applications and point some proposals for

future work.

4

Chapter 2

Background

2.1 Concepts

In this section we will describe the main concepts required for this work. First, we give some notions of

existing grasp models. Grasp stability analysis through force closure is introduced in the second section.

In the third section, we describe notions to perform the collision detection with MoveIt package as some

methods of motion planning explained.

2.1.1 Grasp Model

By definition, a grasp is a set of contact poses between the fingers and the object surface, the set of the

applied forces and material properties, like friction coefficient. Such forces consist of linear and angular

components.

All human grasps belong to a set of the following:

• Pad - the forces between the pads of fingers and thumb held the object.

• Palm - When the opposite force of the fingers, is against the palm.

• Side - The object is held between the thumb and the side of the index finger.

A grasp has some properties:

• The ability to resist to any external force. We want, given a certain set of forces to be applied on

an object, the forces generate a opposing wrenches and compensate each other (force closure

2.1.2).

• The ability to, dexterously, manipulate and re-grasp the object. We must be able to change the

position of some fingers in order to re-grasp the object and perform some task.

2.1.1.1 Human Grasp Choice

Grasps are divided in two categories suggested by Napier(1956) [5]: power and precision. Power cat-

egory is distinguished by large areas of contact between the surface of the fingers and the object. It is

5

chosen when we want stability and security but it has a little or none ability to impart motions with the

fingers. When we want to consider sensitivity and dexterity we choose a precision grasp. In precision

grasps, the object is held with the tips of the fingers and thumb.

Looking at the tree in diagram 2.1 , we can see that, the first choice is between power and preci-

sion. In the next level, our choice is based on geometric and task related. Both task requirements and

geometry information are important to select a grasp.

If we choose precision grasps, the question to be made is about the shape of the object: Is the object

prismatic or circular? After answering this question, we move to the next level. Then pick the type of

grasp according to the task to be done.

If we choose power grasps, the first question is different from the question for precision. Here, we

ask about the object need to be supported or clamped to sustain forces from all directions. If it needs to

be supported, we pick a non-prehensile hook grasp or palmar support. If it needs to be clamped, then it

might need a prehensile grasp in which the fingers or the palm confine the object.

Grasp

Power

Prehensile

Circular

Disk Sphere

LateralPinch

Prismatic

LightTool

HeavyWrap

LargeDiameter

SmallDiameter

AdductedThumb

MediumWrap

Non-Prehensile

Hook (PlatformPush)

Precision

Compact

Circular

Disk Sphere Tripod

Long

Prismatic

(opposed thumb+ finger(s))

Thumb-3Finger

Thumb-4Finger

Thumb-2Finger

Thumb-IndexFinger

Diagram 2.1: Tree with the taxonomy and manufacturing of grasps adapted from [6].

Napier suggest that objects should be grasped depending first, on the task and then, on their shapes,

because with one simple object we can perform multiple tasks. During the course of a single task with a

single object, the hand can adopt different grips to adjust to changing force/torque conditions.

For this work we will adopt precision grasps because we will need enough degrees of freedom for

manipulation task and single-handed operations.

2.1.1.2 Analyzing The Contact Model

A contact model [7] is based in characteristics like force and torque, that can be applied to an object,

allowing relative motions of the bodies in contact. It depends on friction coefficients and other materials

properties of both parts(finger and object).

6

There are three most known contact models [8]. In table 2.1 there is a list of components (contact

forces, torques, moments and velocities) that are transmitted through each of the three contact models.

This table was made based on the information present in the text of the [8].

When the point-contact-without-friction (PwoF) model is used, the friction between the finger and the

object is nearly zero and the contact surface is very small.

The hard finger (HF) model also known as point-contact-with-friction is when there is a significant

contact friction, but the contact patch is small, so that no appreciable friction moment exists.

The third model is the soft finger (SF). It considers that the surface friction and the contact patch are

large enough to generate significant friction forces. A friction moment about the contact normal is also

transmitted. It is the most complete model that allows forces to be applied in a cone (see Friction Cone)

about the surface normal.

In table 2.2, we have some characteristics about the different contact models. The notions of friction

cone and wrenches will be explained in the next subsections.

Table 2.1: Components transmitted and not transmitted through the contact in each contact model.

Contact ModelComponent Point-Contact-Without-Friction Hard Finger Soft Finger

Normal Contact Force Transmitted Transmitted TransmittedContact Frictional Force Assumed to be Negligible Transmitted TransmittedNormal Contact Moment Assumed to be Negligible Not Transmitted Transmitted

Contact Frictional Moment Assumed to be Negligible Not Transmitted TransmittedNormal Translational Velocity Transmitted Transmitted Transmitted

Tangential Translational Velocity Not Transmitted Transmitted TransmittedNormal Angular Velocity Not Transmitted Not Transmitted Transmitted

Tangential Angular Velocity Not Transmitted Not Transmitted Transmitted

Relative to the contact model, we will adopt an extended version of soft finger contact model. This

version considers not only one point of the contact but all points belonging to the contact. Each point is

represented using the soft finger contact model.

Now we will introduce the definition of friction cone mentioned in table 2.2.

2.1.1.3 Friction Cone

With soft finger contact model, we can apply forces in a cone. This cone is named as friction cone [4],

[1] and it is built aligned with the normal to the surface of contact.

The model chosen, soft finger, is a more realistic model and allows the forces to be applied in the

friction cone and the torques about the contact normal. The friction model to be used is Coulomb’s

friction model, [4]. The formula for Coulomb’s friction model when we have soft finger contact model is:

FCci =

{f ∈ R4 :

√f21 + f22 ≤ µff3, f3 ≥ 0, |f4| ≥ γf3

}(2.1)

where γ(γ ≥ 0) is the torsional friction coefficient, f4 is the torque magnitude along the contact normal

direction, f1, f2 and f3 represent the force along x, y and z respectively, µf is the static coefficient of

7

Table 2.2: Table with the most common contact models. Adapted from [4].

Contact type Features Wrench basis Friction Cone

Frictionless point contact Allows forces to be appliedin the contact normal.

001000

f1 ≥ 0

Point Contact with Friction(Hard Finger)

Allows forces be appliedin a cone centered in the

contact normal.

1 0 00 1 00 0 10 0 00 0 00 0 0

√f21 + f22 ≤ µff3,

f3 ≥ 0

Soft Finger

Allows forces be appliein a cone centered in the

contact normal.Allows torques about

contact normal.

1 0 0 00 1 0 00 0 1 00 0 0 00 0 0 00 0 0 1

√f21 + f22 ≤ µff3,

f3 ≥ 0, |f4| ≥ γf3

friction. In table 2.2 we can see the main differences between the most common contact models and to

know more about contact models, consulting [4].

Looking at table 2.2, in the line corresponding to the soft finger contact model, we have a wrench

basis (matrix) where the three first lines, when multiplied by the vector of the forces and torques in all

directions, are related with the forces in the three axis x, y and z and the three last lines correspond to

the three components of the torque. Once this model allows torque only about the normal of the contact,

coordinate z, it only has one non-null line corresponding to the torque about z axis.

To analyze a grasp from the set of wrenches that represents it, we compute the friction cone sampled

over its outer limits and we obtain equation (2.2), with the normal force being unit of magnitude, f3 = 1.

Hereupon, the equation (2.2) provides us the base framework for the grasp formulation.

wkci =

µf sin(θk) µf sin(θk)

µf cos(θk) µf cos(θk)

1 1

0 0

0 0

γ −γ

, θk ∈ [0, 2π] (2.2)

In image 2.1 there are two examples of approximation of friction cones by sampling the base of the

cone with a discretization level of four in the example on the left and a discretization level of eight on

the right example. This discretization of friction cone accelerates the process, because it approaches a

cone by a pyramid and instead of calculate the all perimeter of the cone’s basis, it calculates a polygon

base.

In figure 2.2 it is an example of a two-fingers grasp of a box. The coordinates z of each finger

(represented with a cylinder) is the direction in which the friction cone will be computed.

8

Figure 2.1: Two examples of friction cone. The example on the left is a friction cone with four iterations.The example on the right has six iterations. Image source [4].

Figure 2.2: Example of a grasp with two fingers of a box. Image source [4].

2.1.1.4 Grasp Representation

Since we already introduce notion of grasp and some of its varieties, some concepts about contact

model and friction cone, we will show how the grasp is mathematically represented.

All contacts are defined as sets of wrenches (forces and torques), positions and normal. We just

need to transform all coordinates into a common reference frame so we can treat data correctly.

The set of all transformed wrenches defines a grasp and it is designated as grasp map.

The grasp map is represented by the matrix G [1]:

Gi =

Rci 0

[pci ]xRci Rci

ωci i ∈ [1, ..., n]. (2.3)

where Rci is the rotation matrix from one reference frame to another reference frame and [pci ]x is

the skew-symmetric matrix of pci which is defined by:

px =

0 −p3 p2

p3 0 −p1−p2 p1 0

, (2.4)

9

and n is the total number of contacts.

One important note is that, the choice of the reference frame will not influence the final result which

allows us to use any framework to transform the coordinates. Although, we will use as global reference

frame, the palm of the hand.

The resulting grasp map is:

G =[G1, ..., Gn

]. (2.5)

With the grasp map defined, we have the tools to compute force closure test to validate the stability

of a grasp.

2.1.2 Force Closure

Force closure is a binary test to evaluate the grasp stability. A grasp is stable and so a force closure

grasp if the grasp can resist any applied wrench. Mathematically speaking, a grasp is force closure if

there is a combination of contact forces, fc such that, when applying an external wrench to the object,we:

Gfc = −we, (2.6)

where fc is a vector of the forces of each contact in friction cone:

fc =

fc1

fc2

.

.

.

fcn

, fci ∈ FC (2.7)

and n is the number of contacts that compose the grasp.

We can easily evaluate if a grasp is a force closure grasp by analysing the convex hull of the grasp

map, ConvexHull(G). The convex hull of G is the minimum convex region spanned by G on the wrench

space.

In figure 2.3 it has represented the wrench space (force, torque). It has only 2 dimensions since, if

we discretize all force components, with the torque it would be a 4D space. So, with the wrench vectors,

corresponding to each contact, in it, we have the minimum convex region that contains all wrenches that

represent the grasp, in green, as known as convex hull of the set of wrenches. In this example, we can

say that the grasp is force closure since the origin is inside the convex hull.

Since each contact between one finger and the object has many points of contact, the convex hull

of all points generates a patch that will be the surface of contact. We will use all points of the surface

contact to obtain the force closure result.

10

Figure 2.3: A set of five wrenches represented in the wrench space (abscissa- force; ordinate-torque).Image source [1].

2.1.3 Collision Detection

One of the most essential parts of this project is the collision detection. When some finger reaches the

object, it needs to avoid collision during the motion and, at the same time, we need to promote a collision

between fingers and an object to create the contact of grasping the object. For this purpose there is a

library called FCL - Flexible Collision Library [3] present in ROS through MoveIt package.

2.1.3.1 FCL - Flexible Collision Library

This library was developed with many algorithms for collision detection and distance computation. To do

that, the objects must be represented with simple shapes to accelerate the whole process. In this section

we will present a topic where we briefly explain the object representations methods, a topic with all the

techniques to detect collisions and compute the distance before any collisions happen and another topic

explaining hierarchical collision detection.

Object Representation A generic object may have lots of details and be non-convex which can slow

down its analysis. To deal with this problem, FCL creates simplified representations of objects based

in triangle meshes or point clouds. We will not study point clouds, just triangle meshes. To accelerate

collision detection and distance computations, each object is approximated by a finite set of convex

components named bounding volume (BV) [9]. By definition, BV is the smallest volume, regarding simple

geometry like spheres, boxes, cones or cylinders, which contains the object inside. The larger amount

of components, the better is the approximation to the original shape of the object and, at maximum we

can have one BV for each triangle of the mesh.

11

There are many types of bounding volumes and mostly FCL’s bounding volumes are boxes whose

faces, edges and vertices delimit the object. There are bounding spheres which surface delimits the

object boundaries too that are less used. Next, we will describe some methods used in FCL to represent

objects.

AABB - Axis-aligned Bounding Box

With AABB [10], the BV of an object is created by approaching its edges, aligned with world axis, to

the object until it reaches the object boundaries like we can see in figure 2.4a).

Figure 2.4: a)AABB , b)OBB. Image source [10]

One Disadvantage is when we are dealing with rotational objects and at each rotation, the bounding

volume has to be recomputed since this representation does not allow rotation. It is more indicated to

fixed or with linear motion objects. One advantage is the collision detection is faster because it is a

simpler representation.

OBB - Oriented Bounding Box

Representing objects with OBB [10], the BV of an object is created in similar way than AABB. After

finding AABB, the box rotates and adjust its faces, edges and vertices to the object boundaries until the

pose in which the BV volume is smaller like in figure 2.4b) where we can see that the empty space inside

the bounding box is smaller than 2.4a) due to rotation applied.

This representation is more recommended to articulated bodies since it follows the rotation of the

object but as consequence is slower than AABB.

kDOP - Discrete Oriented Polytope

kDOP method [11] is a generalization of AABB in object representation where k symbolizes the num-

ber of direction in which each pair of planes approaches the object.

kIOS - Intersection Of k Spheres

This method kIOS [12] results from intersecting a k number of spherical surfaces whose delimit the

object boundaries. Spheres distribution is related with the dimensions of the object: the largest dimen-

sion is delimited by the first sphere. The second larger dimension is delimited by the second sphere in

12

case it exists and so on. The value of k is related with objects dimensions: if dimensions are similar

(the ration between them smaller than 1.5), then k = 1; if only two dimensions are similar, then k = 3,

otherwise, k = 5 as we can see in algorithm 1.

In figure 2.5 since we have a 2 dimensional case, there is only two possible value for k depending

on similarity between those dimensions. Since the two dimensions of the object are different, k will take

the value 3.

Figure 2.5: Example of kIOS applied in yellow object. The curves in blue means the 3 spheres thatorigins the bounding volume(in red). Image source [12].

Algorithm 1 kIOS algorithm

1: ratio← COMPUTE RATIO(object.dimensions)2: if ratio < 1.5 then3: n sphere← 14: else5: ratio← COMPUTE RATIO(object.smaller dimensions)6: if ratio < 1.5 then7: n sphere← 38: else9: n sphere← 5

10: end if11: end if12: bv ← COMPUTE IOS(n sphere, object.dimensions)

Collision Detection in Simple Objects Collision detection is made by compare both BV of objects.

To do so, FCL has many techniques such those applied to static objects (discrete collision detection) in

which is not take into account the motion of the objects. There are other techniques which consider the

motion over time (continuous collision detection).

The queries in FCL are : discrete collision detection, depth penetration, collision detection between

multiple objects, continuous collision detection and distance computation.

13

Discrete Collision Detection and Depth Penetration

It consists in detecting collisions between two BV. We call it discrete because only detects collision in

a moment of time and not during a full motion. To compute this detection, FCL is based on GJK (Gilbert-

Johnson-Keerthi) [13], EPA (Expanding Polytope Algorithm) [14]. Both based on Minkowski Difference

[13].

Minkowski Difference

The Minkowski Difference method creates a new object from every point of one object, subtracts every

point of other object. The resulting object is a convex hull of the set of such resulting points. As we can

see if figure 2.6, if two objects are intersecting, the result of applying Minkowski Difference contains the

origin and, if two objects do not collide like in figure 2.7, the distance between them is the distance from

the object to the origin.

Figure 2.6: a)Dois objectos em colisao. b) Polıtopo e pontos resultantes da Diferenca de Minkowski.

GJK - Gilbert-Johnson-Keerthi

GJK is a method based on Minkowski Difference but, instead of using all points from Minkowski

difference, it uses the pair of points (a, b) with a ∈ ObjectA and b ∈ ObjectB closest to each other

with gives the desired result by computing the distance between the origin and the result of Minkowski

Difference since ‖a− b‖ = d(A,B).

Each iteration constructs a simplex within the polytope resulting of Minkowski that lies nearer the

origin than the one built in previous iteration. In algorithm 2, W is a set of vertices of the simplex

belonging to that iteration and it is initialized as empty, vk is the point in the simplex nearest the origin

and it is initialized as an arbitrary point of Minkowski Difference resulting polytope. Then GJK generates

a sequence of simplices with w = s(A−B) in which s(A−B) is the support function [13]. The support

14

Figure 2.7: a)Dois objectos sem colisao. b) Polıtopo e pontos resultantes da Diferenca de Minkowski.

function of a set, maps a vector v to a specific point of the same set called ”support point”. This point is

the most extreme point of an object in a certain direction v.

Algorithm 2 GJK algorithm

1: v← ”arbitrary point in C = A - B”2: W ← ∅3: µ← 04: close← False5: while close == False and v 6= 0) do6: w← s(A−B)(−v);7: δ ← v.w

‖v‖ ;

8: µ← max{µ, δ}9: close← ‖v‖ − µ ≤ ε;

10: if not(close) then11: v← υ(conv(W ∪ {w}));12: W ← ”smallest X ⊆W ∪ {w} such that v ∈ conv(X)”;13: end if14: end while15: return‖v‖

EPA - Expanding Polytope Algorithm

EPA algorithm [14] has as main purpose computes the depth penetration when there is a collision be-

tween two objects. To do that, EPA uses the simplex containing the origin resulting from GJK algorithm.

Geometrically speaking, EPA expands that simplex to obtain the Minkowski Difference edge closest

to the origin. Since the penetration depth is the point in Minkowski Difference polytope boundary closest

to the origin, EPA expands the initial simplex by adding vertices that lie on the same boundary, by calling

support function also used in GJK. In each iteration, EPA selects the polytope facet closest to the origin

and subdivides it adding vertices using support function. Some examples can be found on [14].

15

Figure 2.8 shows us an example of EPA application with the initial simplex in purple and polytope

from Minkowski Difference in green. Algorithm 3 explains how the expansion is done in which spolytope(p)

is the support function that returns a point in polytope boundary. The function CalculateDepth(polytope)

takes the expanded polytope, find the closest point in polytope boundary and returns the distance to the

origin and the orientations which gives us the penetration depth and the penetration vector.

Figure 2.8: Example of EPA with 2 iterations: a)First edge closest to origin selected, b)First edge closestto origin expanded. Second edge closest to origin selected, c)Second edge closest to origin expanded.A Edge of A−B polytope is the closest to the origin. Expansion terminated.

Algorithm 3 EPA algorithm

1: polytope← GJK simplex2: closest← polytope.closest edge3: while closest /∈ (A−B)polytope do4: spolytope(p)5: polytope.add vertice(p)6: closest← polytope.closest edge7: end while8: [lengthdepth, vectordepth]← CalculateDepth(polytope)9: return [lengthdepth, vectordepth]

Collision Detection Between Multiple Objects

In a environment with so many objects, it becomes mandatory that it should exist a previous screening

which eliminates all pair of object that are not in collision. For that, FCL has the Sweep and Prune

algorithm [15].

SaP - Sweep and Prune

This algorithm verifies the existence of any collision in each axis at a time. First, SaP tests each pair

of objects for overlapping in xx axis and if they overlap, it puts them into a first list of possible colliding

objects. The next step is verified if the pairs in the first list collide in yy axis. If the pairs of objects present

in the first list also collide is yy axis, they will be allocated in a second list. Finally, SaP tests all left pair

in second list. If they collide in zz axis, it puts them into a final list of pairs of objects that actually collide.

16

In figure 2.9 we can see an example of object that seems to be in collision but just when view from xx

axis. That pair of objects will go to the first list but they do not pass for second list. The example shown

in figure 2.10 shows that pair of objects is colliding not only in xx axis but in yy and zz axis too. So that

pair will pass for all three lists and it will be classified as pair of objects in collision.

Figure 2.9: a)Overlapping objects seen from xx axis, b)Non-overlapping objects seen from yy axis,c)Non-overlapping objects seen from zz axis, d)Set of pairs of objects: First contains pairs which over-lapping on xx axis; Second contains pairs which overlapping on xx and yy axis; Third contains pairswhich overlapping on xx, yy and zz axis.

Figure 2.10: a)Overlapping objects seen from xx axis, b)Overlapping objects seen from yy axis,c)Overlapping objects seen from zz axis, d)Set of pairs of objects: First contains pairs which over-lapping on xx axis; Second contains pairs which overlapping on xx and yy axis; Third contains pairswhich overlapping on xx, yy and zz axis.

Continuous Collision Detection And Distance Computation

With FCL it is also possible obtain distance between two objects which are not in collision with Con-

servative Advancement [16]. We can also get the time instant in which such collision will happen if the

motion of the objects keep unchanged.

CCD - Continuous Collision Detection

Some continuous collision detection algorithms based on linear motion interpolation between two

different objects configurations. Those interpolations are reflected on a representation of motion named

swept volume [17].

SV - Swept Volume

The idea of swept volume is, given the motion of a certain object, draw that object as one during its

movement (see algorithm 4). In figure 2.11 there is an example of two objects in motion and the result

of they being swept through their motion. We can see also the moment of collision between them.

17

Algorithm 4 SV algorithm

1: SV ← ∅2: height← BV height3: width← BV width4: depth← getTrajectory(Objmotion)5: depth← depth+BV depth6: SV ← getV olume(height, width, depth)7: returnSV

Figure 2.11: Example of two SV and their collision.

CA - Conservative Advancement

With conservative advancement [16] techniques we are able to estimate the instance of time when

some collision is going to happen. In algorithm 5 adapted from [16], we can observe the pseudo code

of the CA algorithm. The leaf nodes refer to bounding volume hierarchies that are based in tree which

nodes are bounding volume of object parts and it will be explained in topic Hierarchical Collision Detec-

tion. The method computes upper limit of time of contact that is the instance of time when collision is

going to happen. This computation is made by advance repeatedly the object through is motion with a

∆t time step.

Separation Distance between pairs of objects

With EPA algorithm we get the penetration depth. Similarly, in distance computation we intent to

find the distance in which two objects are from each other. Beside existing many methods to find that

distance like in [18], FCL based on EPA and Minkowski Difference polytope.

In figure 2.7, the two objects do not collide and so the (A−B) polytope does not contain the origin. For

instance, for two objects that collide, the penetration depth is the distance from the origin to the closest

point of polytope boundary to the origin. In two objects that do not collide, the separation distance is the

distance from the origin to the closest point of the boundary to the origin.

Hierarchical Collision Detection With hierarchical collision detection, the object is represented through

a tree is which each node represents one part of the object and, with such representation, we are able to

know more strictly in which part of the object, the collision happened. Such hierarchy is called bounding

volume hierarchy, BVH.

18

Algorithm 5 Conservative Advancement algorithm

1: if number of CA iterations > Rmax and dcur < Dthreshold then2: w = 1;3: end if4: if n1, n2 are leaf nodes then5: d = Distance(n1, n2);6: if d < dcur then7: dcur = d;8: end if9: ∆t = CalculateCAStep(d, n1, n2);

10: if ∆t < ∆tcur then11: ∆tcur = ∆t;12: end if13: return∆tcur, dcur14: else if n1 is not a leaf node then15: A← n1.leftchild; B ← n1.rightchild; C = D = n2;16: d1 = Distance(A,C); d2 = Distance(B,D);17: else18: A = B = n1; C = n2.leftchild; D = n2.rightchild;19: d1 = Distance(A,C); d2 = Distance(B,D);20: end if21: if d2 < d1 then22: if d2 < wdcur then23: returnC2A(B,D, dcur,∆tcur, w);24: else25: ∆t = CalculateCAStep(d2, B,D);26: if ∆t < ∆tcur then27: ∆tcur = ∆t;28: end if29: end if30: if d1 < wdcur then31: return C2A(A,C, dcur,∆tcur, w);32: else33: ∆t = CalculateCAStep(d1, A,C);34: if ∆t < ∆tcur then35: ∆tcur = ∆t;36: end if37: end if38: else39: if d1 < wdcur then40: return C2A(A,C, dcur,∆tcur, w);41: else42: ∆t = CalculateCAStep(d1, A,C);43: if ∆t < ∆tcur then44: ∆tcur = ∆t;45: end if46: end if47: if d2 < wdcur then48: return C2A(B,D, dcur,∆tcur, w);49: else50: ∆t = CalculateCAStep(d2, B,D);51: if ∆t < ∆tcur then52: ∆tcur = ∆t;53: end if54: end if55: end if

19

BVH - Bounding Volume Hierarchy In algorithm Conservative Advancement we saw that object

might be represented as trees called bounding volume hierarchies. The main node of those trees is

a BV with all object inside. That node is divided into two children nodes in which each node will be a

BV with the relative part of the object inside. The children of the next node will be divided just like the

previous one and they will contain the BV of correspondent parts of the object. In algorithm 6 adapted

from [11], we can see the process to detect collisions in such trees. Figure 2.12 has an example of a

BVH of a articulated object by applying AABB and another one by applying OBB.

Figure 2.12: a)BVH result of an articulated object. b) BVH based on AABB(see 2.1.3.1). c) BVH basedon OBB(see 2.1.3.1). Dashed borders represent BV boundary. Continuous lines mean original objectboundaries. Adapted image from [19]

As time goes by, the objects can move. To take the motion into account, the position of the object is

actualized as objects are moving. Those updates can be done by two different ways:

1) The first one is the usual way in which it builds a new tree for each iteration of motion.

2) In the second one, it fits the existent BVH by updating the position and orientation of the object.

After updating the trees, FCL proceeds to develop a BVTT, bounding volume test tree [20] similar

to algorithm 6 that will contain the collision information of each node of one tree comparing with the

other node of the other tree like we can see in figure 2.14 which is the BVTT of the two object A and B

presented in figure 2.13.

BVTT process is used mostly because if the objects do not collide, it is only need to verify the first

node of the BVTT that accelerates the all collision detection process.

2.1.4 Motion Planning

To plan the motions of each finger, it is necessary resort algorithms to do motion planning. For this

purpose we use the Open Motion Planning Library from Moveit which has, as default algorithms RRT

(Rapidly-Exploring Random Trees ), in case the state space has no default projection and, if the state

space has a default projection, which is going happen if it is used any built-in state spaces, KPIECE

algorithm.

20

Figure 2.13: a)BVH of object A ; b)BVH of object B.

Figure 2.14: Resulting BVTT of objects in figure 2.13.

2.1.4.1 OMPL - Open Motion Planning Library

Using this library became required since, to manipulate objects, the fingers have to change their pose in

the object. The algorithms in OMPL, RRT and KPIECE are both tree-based planners since they begin

by rooting a tree at the starting configuration of the robot that, in this case, is one finger. The method of

expansion of each method differs but in the end, if it exists a path with no collisions, the algorithm return

that path. For path we mean a sequence of connected possible states that reaches a final state in the

robot state space with all elements collision free.

In OMPL there are two types of algorithms:

- Control-based Planners - Motion planners in which the result is a control law that given the initial

state, the final state and the world state, allows the robot reach the final state take into account possible

changes in world state.

- Geometric planners - in which the result is a predefined trajectory to be executed by an external

controller.

Despite OMPL having lots of algorithms, in the next sections, we will refer some geometric planners

KPIECE, RRT and PRM.

21

Algorithm 6 BVTT algorithm

1: a = bounding volume of A′s tree2: b = bounding volume of B′s tree3: a[i], b[i] are children of a and b, resp.4: TRAV ERSE(a, b) :5: if a or b is empty then6: return7: end if8: if b is leaf then9: if a is leaf then

10: check primitives enclosed by a and b11: return12: else13: for all i do14: if a[i], boverlap then15: TRAV ERSE(a[i], b)16: end if17: end for18: end if19: else20: if a is leaf then21: for all i do22: if a, b[i]overlap then23: TRAV ERSE(a, b[i])24: end if25: end for26: else27: for all i do28: for all j do29: if a[i], b[j]overlap then30: TRAV ERSE(a[i], b[j])31: end if32: end for33: end for34: end if35: end if

KPIECE - Kinodynamic Planning by Interior-Exterior Cell Exploration

KPIECE is a tree-based method which explores the state space by discretizing the state space into

uniform cells with same fixed size. During the exploration, the cells are classified as interior or exterior

cells depending on non-diagonal neighbour cells [21].

• Exterior cell has less than 2n neighbour cells.

• Interior cell has 2n neighbors.

The tree is initialized with a movement of zero duration which consists on the initial state of the

robot. Exterior cells are preferred to interior cells and when all neighbor cells are interior or exterior, the

importance of each cell is the new criterion to exploration. The definition of importance can be found

on equation 2.8 where p is the cell to evaluate, I is the iteration number when p was created, score is

initialized with value 1; S stands for the number of times p was selected for expansion, N means the

number of neighbor cells instantiated at the same level of discretization and C is a positive measure of

22

coverage for cell p. The cell coverage is simply the sum of the durations of the motions in the all tree until

the present iteration. The maximum level of discretization is predefined at the beginning and essentially

increases like shown in figure 2.15.

Importance(p) =log (I).score

S.N.C(2.8)

Each node is defined by the previous state and the movement to go to next cell. The algorithm

finishes when a stopping condition is satisfied such as the final state was reached or all cells was

explored and no solution was found.

Figure 2.15: Example of KPIECE discretization with three levels. Image from [22]

RRT - Rapidly-Exploring Random Trees

In this algorithm, a tree where all nodes correspond to collision free states is built in which the first

node is the initial configuration of the robot. The origin of the node created in each iteration is shown

in algorithm 7 adapted from [23] where xinit is the initial state, Kiterations is the maximum number

of iterations, ∆t is the duration of each movement, Tree is the tree of all motions, xrand is the new

sample of the state space, xnear is the closest node of the tree to xrand, and xnew is the xrand with a

movement u from the xnear during ∆t time. Function RANDOM STATE() return a random state which

will added to the tree after finding the nearest neighbor through NEAREST NEIGHBOR(xrand, T ree)

function. TheNEW STATE(xnear, u,∆t) function creates the state which goes from xnear to xrand with

movement u for ∆t time.

All movements and states are free of obstacles. The simplest movement is a linear trajectory which

connects two states.

This algorithm allows search in non-convex high dimensional spaces and also can deal easily with

23

Algorithm 7 RRT algorithmGENERATE RRT(xinit,Kiterations,∆t)

1: Tree.init()2: for i = 1 to Kiterations do3: xrand ← RANDOM STATE()4: xnear ← NEAREST NEIGHBOR(xrand, T ree)5: xnew ← NEW STATE(xnear, u,∆t)6: Tree.ADD V ERTEX(xnew)7: Tree.ADD EDGE(xnear, xnew, u)8: end for9: RETURN Tree

obstacles, non-holonomic robots and kinematic constrains.

If no final state is defined, the algorithm connects the initial state to all states from state space by

generating random trajectories limited by iterations number. Figure 2.16 shows an example of a solution

for a call of RRT algorithm without final state.

Figure 2.16: Example of RRT method application without final state specification. The image representsthe evolution of the tree during the method from the left to the right. Image from [23].

PRM - Probabilistic Roadmap Tree

PRM is a quite different from the above algorithms since PRM uses random sampling to build the

roadmap of the state space. This roadmap is a set of states in which each state is connected with all

states where there is no object between them. The map created has loops and then call the Dijkstra

algorithm [24], to select the shortest path in found. The phase of map build is the first phase and the

second phase is named as query phase (where Dijkstra algorithm is called).

• Dijkstra Algorithm - This algorithm has as input, a graph and returns the shortest path in the

graph. At first, it creates a list S in which there are the shorter paths that are calculated during the

process. List is initialized empty and at each iteration the edge with lower cost is added to the list

and the vertex is marked as covered. Finally the shortest path in that list is selected and returned.

Figure 2.17 is an example of applying PRM method in a certain environment. The orange path is

returned as the shortest one and it will be connected to the initial and finals states by connecting the

initial state to the beginning of the graph and the final state to the end of the graph.

24

Figure 2.17: Example of PRM method application in an environment with some objects(in gray). Thegraph is composed with gray edges and orange vertex and the solution is the set of orange edges andvertices.Image from [25]

In short, PRM is more expensive since it build an entire roadmap before any information and only

after the map is built it finds the shortest path. In case the final state is relatively near to initial state, it is

not necessary explore all map.

Both RRT an KPIECE are tree-base algorithms but the bigger difference is KPIECE discretizes the

state space into uniform cell and explores it until it reaches the final state while RRT connects random

samples to a tree and it could take more time since the pose of each sample is random. The fastest

algorithm is KPIECE because the that it explores the state space is more balanced and uniform and

only if it needs, it can increase the discretization level, for example, when there is obstacles with lower

dimensions than cells dimension.

2.2 State of The Art

In the state of the art we present some grasping tools already developed as some manipulation tech-

niques in the last section of this chapter.

2.2.1 Grasping

There is already some work developed in robotic grasp. There are grasps performed just with grippers

like with PR2 robot in [26] where the pre-processing to find and execute a grasp is based in hand

sensors: one on the hand and one on each finger of the gripper. That pre-processing is to find the

adequate grasp after identify the shape of the object.

In [27] there are defined some grasps performed with an anthropomorphic hand using synergies. An

anthropomorphic hand is a hand very similar with human hand: same joints, same form, etc. Synergy

25

is the coordinated effect of two or more systems perform a task. In this case, those systems are the

fingers of the robot’s hand and the task is single handed object’s manipulations.

In figure 2.18 we can see eight different grasps that fit in the diagram 2.1, for example: the tripod fits

with precision circular tripod grasp in the diagram while palmar pinch fits with the precision-prismatic-

thumb and index fingers.

Furthermore, in figure 2.18, we have the eight separated grasps and we want the robot write some-

thing and to do so, the selected grasp may not be the writing tripod. Let’s admit that writing tripod is

the only one able to writing. If the pen or pencil is on some surface, the robot need to pick up the pen

and the robotic hand cannot pick the pen with writing tripod at first, so it need to pick the pen with, for

example, palmar pinch and then change some fingers positions to achieve the writing tripod grasp.

Figure 2.18: 8 different kinds of grasps already implemented. Image from [27].

Such grasps are composed by fingers postures. Each posture, p is given by:

p =[θ1, ..., θn

]∈ Rn. (2.9)

where n is the number of joints in the robotic hand. With synergies, each posture is:

p =

b∑i=1

aiei (2.10)

in which {e1, e2, ..., eb} is a linear combination of b hand synergies and {a1, a2, ..., ab} are the weights

of each synergy, ei.

We can also identify the necessary fingers, or joints, needed to perform each grasp and with that we

26

can reach any grasp by verifying the actual joints values and compare to the expected values to reach

the desired grasp.

2.2.2 Manipulation

Currently, there are some work developed in dexterous manipulation considering: object reorientation

[28], fine manipulation [29], finger gaiting [30], [31] and finger rolling with finger gaiting [32]. Finger

gaiting is defined as any periodic sequence of fingers relocations used to achieve a different grasp from

a initial one. It is also known as re-grasping.

In object reorientation, there is mostly used rolling and slipping movements. In [28], there was used

large scale objects and rolling fingers was allowed. The format of the manipulators was three-fingers

gripper (hemispherical fingertips).

In [29] the authors implement finger gaits for turning a sphere with a three-fingered robotic hand by

teaching the sequence of motions required. It builds a RRT tree in which two vertices are adjacent only

if there exists a finger switching between the two grasps from the two vertices. This property allows

re-grasp planning to be formulated as a graph search.

Similarly, [30] shows us a way to orienting a sphere using a 3-fingered manipulator by finger gait also

using RRT.

The closest paper to our approach is [33] which deals with motion planning for multi-fingered hand

manipulating object capable to compute object and fingers trajectories as well as fingers relocation on

the object. It connects configurations of the subspace where the object is grasped into a PRM graph and

its paths are computed using RRT method. The main characteristics are the use of four fingers grippers

like in figure 2.19 and the use of hard finger contact points.

Like previous methods, [31] also uses RRT techniques to develop a finger gait planner to accomplish

objects manipulation. A formulation based on hybrid automaton is used, named finger substitution.

Such formulation based, initially on a three-fingered hand, like in figure 2.20, with two fingers holding

an object and the third does not move. When the grasp need to be adjusted, mainly because one or

both fingers reached its limits to grasp the object in force closure way, the third finger creates a contact

with the object so one of the other fingers can be removed from the grasp, creating a new one. Here,

with the new grasp, the manipulation continues until the goal pose (position + orientation) is achieved

or some other constraint makes the motion stop. In this approach does not exist rolling neither sliding

motions, and the contact model used is hard finger model contact. Both the object and the fingers are

rigid bodies. Geometries of the object and the fingers are known.

Some situations may happen like [32] where it is admitted that all fingers are needed to perform

the initial force closure grasp so gaiting will not possible to apply unless it uses rolling and/or sliding

first to move fingers contacts in object surface. So, at least, one finger can be dispensed from the new

force closure grasp. In the referred paper, it is used rolling and finger substitution. It is used hard finger

contacts for three finger grasps and soft finger contact for two finger grasps. Robotic hands format is a

three-fingered gripper similar to [30].

27

Figure 2.19: Example of an object reorientation by finger gaiting. Image from [33].

Figure 2.20: Example of a sphere manipulated with finger substitution. Image from [31].

There is previous work on dexterous manipulation in which is used not only fingers for manipulation

but also palm to accomplish the task of pick up an object a let the object roll from the palm to the

fingertips so the object is deposited on a surface ([34]). This approach uses an anthropomorphic hand

model that provides stable grasps and allows non-prehensile manipulations and so the exploration of

object motion with fewer actuated degrees of freedom to manipulate the object it is possible. We can

see an application example in figure 2.21 where the object is grasped; there is some manipulation to

drop the object statically on the palm and with continuous rolling movement of the object, move the

object to closer from the fingertips.

The article [35] proposes switching from one grasp configuration to other by a 4-fingered hand using

hard finger contact model and similar to previous methods, it is based on switching graph that allows

re-grasping planning. It takes into account non-convex polygons but do not consider curve objects. They

also consider rolling motion of fingers. One of the future work proposed is extend their approach to three

28

Figure 2.21: Example picking and dropping an object. Image from [34].

dimensional case.

This last approach is very similar to our approach since we intend to explore re-grasping feature with

robotic hands. In our case we want to use 5-fingered anthropomorphic hand.

Similarly to what happens in finger gaiting, we intend to move some fingers to different contact

positions on the object to reach a new grasp. Due to having 5-fingered dexterous hand, we barely have

grasps where all five fingers are required to have a force closure grasp. We will not need to resort to

rolling neither sliding to make a grasp forced closure with less fingers.

The manipulation that we intend is basically grasp an object and achieve some other grasp so we

can perform some task with the grasped object. We will not consider fingers rolling neither sliding in

fingers repositions. However, during the time when the object is used to do some task, some rolling

might happen but we will not address explicitly those cases.

Our approach has a bit of each approaches referred above since we want to re-grasp objects with

fingers relocation but with 5-fingered dexterous hand. We also want to give the ability to hold the object

so it can be used with its own purpose.

29

30

Chapter 3

Outline of the Proposed Algorithm

In the literature there exists some algorithms that generate grasps that allow the robot to pick up objects.

With this work, we intend to give the ability to robots change the position of the fingers, with the object

held. The purpose is to reach some other grasp to use the object.

3.1 Finger Gaiting Algorithm

First of all we should define the required inputs of this algorithm. We need a robotic hand, a set of, at

least, two force closure grasps, one as the initial pose and another as the final pose and an object to

perform those grasp poses. The object should be compatible with the selected grasps so the re-grasp

task can be accomplished.

Given an initial force closure grasp, we start by the contact acquisition so we can identify which

fingers are removable from the current grasp.

This identification in described in algorithm 8.

Algorithm 8 Identify Removable Fingers

1: fingersnames ← ”names of fingers from topic positions normals”2: for i = 1 to numberfingers do3: Vpositions ← ”positions of i-th finger from topic positions normals”4: Vnormal ← ”normal of i-th finger from topic positions normals”5: end for6: if ForceClosure(Vpositions,Vnormal) then7: for fingeri = 1 to numberfingers do8: Vnormalaux ← Vnormal ”without normal from i-th finger”9: Vpositionsaux ← Vpositions ”without positions from i-th finger”

10: if ForceClosure(Vpositionsaux ,Vnormalaux ) then11: Vremovable ← fingeri12: end if13: end for14: end if15: publishVremovable in removable contacts

It tests finger by finger, which can be removed from the grasp without loose the stability. If one of the

fingers is not touching the object, that finger is automatically a removable one.

31

Identified those fingers, we start with the re-grasp task. This re-grasp task is based on finger gaiting.

The gait process is done as shown in algorithm 9.

At each contact changing, we update the force closure result so we can have a force closure grasp

all the way. At each true result of force closure, the removable fingers might change, since they are

updated at each finger gait.

Algorithm 9 Identify Removable Fingers

1: while current pose 6= final pose do2: removable fingers← ”names of fingers from topic removable contacts”3: if removable fingers 6= ∅ then4: if ∃removable fingers(i) ∈ fingers to reposit(j) then5: if finger done(j) = 0 then6: Lift finger(removable fingers(i))7: Reposition finger(removable fingers(i))8: Land finger in obj(removable fingers(i))9: finger done(j)← 1

10: end if11: end if12: end if13: end while

In figure 3.1 we have a flowchart to illustrate the sequence of actions done by the algorithm.

At each update of the force closure test and the removable fingers set, the fingers that could not be

relocated in previous iterations and now can, are selected to be relocated until they reach the final pose.

The algorithm gives priority to reposition the needed fingers for the final grasp pose. After all the

fingers from the final grasp are in the corresponding final poses, the fingers not required are lifted. The

final pose is reached and th algorithm ends.

As outputs from this algorithm we have the finger gaits that lead each of the fingers, from the initial

grasp, to the final grasp. Each finger is relocated at a time so we can have a forced closure grasp in

every iteration of this algorithm. The final result will be the robotic hand holding the object with the

configurations of the final grasp.

32

Figure 3.1: Flowchart illustrating the proposed algorithm (see text).

33

34

Chapter 4

Implementation

4.1 ROS Development Platform

4.1.1 Brief Explanation

ROS, Robot Operating System [36], is an open-source flexible framework like a middle ware to write

robot software. It has many tools, libraries and conventions that simplify the task of creating robot

applications across many robotic platforms with abstraction of hardware, device drivers. It is very robust

since it has cross-language interoperability.

The ROS file system is leveled in packages and manifest. Packages are the folders in which have

libraries, messages, services and nodes. Manifest is a description file of the package that defines the

dependencies and it has information about the package.

ROS is known by its architecture and communication tools. The fundamental concepts of ROS are

nodes, messages, topics and services.

Nodes are processes that perform computation, also known as software modules. Packages can

have one or more nodes that communicate with each other by sending or receiving messages through

topics. Messages are typed data structures that are mostly composed of a header and primitive data type

components (single, arrays, arrays of other messages). A topic is where the messages are published

and the nodes can subscribe topics which provide information that was published by other node. In figure

4.1 there is an example of communication between some nodes and topic by publishing and subscribing

topic’s information.

To communicate between nodes, ROS also has service client models that are defined by a pair

of message structures: one for the request (client) and one for the reply(service). The service must

be always active and is waiting for the client sends a request message to use the service and then the

service reply to that request. Each client has only one service associated unlike topics that allow multiple

subscriptions.

ROS also has a transform package named tf used to acquire transform data from one reference

frame to another. The figure in appendix 6.2 illustrates a tree which is dynamically constructed by ROS

to allocate all transforms.

35

topic 1

node 1

topic 2

node 2node 3

topic 3

subscribing

publishind

subscribingsubscribing

publishing

publishing

Figure 4.1: Simple example of topics and nodes in-teraction (publications and subscriptions).

base link

torso

neck

head

right shoulder

right elbow

right wrist

Figure 4.2: Example of a ROS tf tree. Each edgerepresents a transform from the parent node to thechild node.

4.1.2 Software Architecture

From communication tools available in ROS we used topics communications and service clients.

One of the service clients was used to make a request from force closure code which was written in

Python language. The server is always active and when needed, the client sends a request to obtain

the result of force closure test provided by the server. The server itself is a node that has the code to

compute the result while the client can be a node or may be associated in any node since the node only

has one service.

The other service client was used to acquire the updated values from the joints when the hand is

holding the object with the first grasp.

The communication by topics was used to acquire information about contact links in Gazebo topics

(see 4.2), publish and subscribe transformed coordinates of such contacts, publish the values from the

angles of each joint, publish joint values to fingers joints controllers, publish and subscribe the fingers

that are available to remove, given a certain grasp.

We use tf package to acquire and to apply transforms to contacts’ positions and normal coordinates

when acquired from Gazebo’s topics. In appendix 6.2 there is the tf tree from the model used, where

36

we can see all sequences from the right hand to each fingertip, the model of the box used and the base

used so the hand can be fixed.

It was developed based on the diagram of figure 4.3 where ellipses stand for nodes. Rectangles

represent packages. Diamond stands for the Force Closure and Joint Angles services respectively. The

arrows represent communications. Continuous arrows with one direction mean from publisher node to

subscriber node topics. Arrows with both directions stand for service calls and answers. Dashed arrows

with one way mean consult a package to help in some computation.

Software Architecture (ROS)

19Instituto Superior Técnico

ContactAcquisition

Identify Removable

Fingers

Gazebo

Gazebo MoveFingers

JointsAngles Service

ServiceCall

FingersClosing

Force Closure Service

ServiceCall

TF

MoveIt

CollisionDetection

ROS

/contacts/positions

&normals

/removable fingers

/joints angles

/joints values

/joints values

ROS Nodes

ROS Packages

ROS Services

ROS Topics

Labels:

03/12/15 Figure 4.3: ROS structure of this algorithm (see text).

In the figure 4.3, we can see the main components of the implemented software architecture. The

red part is the package MoveIt from ROS used to do the collision avoidance. Blue part has the ROS

parts belonging to this algorithm. The orange part has the code to do the first grasp pose and it uses

the MoveIt package.

The major parts were developed under ROS although we need to acquire the contact information

from Gazebo. The green part represents the Gazebo tools: simulator and the topic with the information

about contacts.

37

4.2 Gazebo Simulator

4.2.1 Brief Explanation

Gazebo simulator, documented in [37], is designed to simulate a 3D dynamic multi-robot environment

capable of recreating the complex worlds with many physics parameters and light conditions that can

be adjustable by the user. Gazebo server reads URDF or SDF files formats to generate the world and

populates it according to what is in the files. These files extensions are ”.world” to load world parameters,

”.sdf” to introduce robot’s models into the built world. The robots are dynamic structures composed of

rigid bodies connected by joints. Angular and linear forces can be applied to generate movement and

interaction with simulated world. All simulated objects have mass, velocity, friction and other attributes

that allow them to behave realistically when pushed, pulled, knocked over, or carried. Simulated bodies

can have sensors, cameras or any interfaces associated to facilitate data flow. They can have none or

more joints to connect bodies and so to form dynamic and kinematic relationships. There are three kinds

of joints: fixed just to connect two bodies, planar joints so translation motion can happen and rotation

joints so a body can rotate like happen in articulated bodies. Joints also can exist like motors where the

force can be applied to generate motion due to friction between the two bodies.

Physics engine used by Gazebo, simulates dynamics and kinematics associated with articulated rigid

bodies and has features as collision detection, mass and rotational functions.

Besides simulation, Gazebo also provides visualization tools so the users can understand in a better

way the environment and interact through mouse and keyboard.

To make information data from simulation environment easier to access, Gazebo allows building plu-

gins. These plugins provide access to the respective libraries without using the communication system.

There are five types of plugins allowed: world, model, sensor, system and visual plugins. These are the

five different Gazebo components.

A world plugin is attached to a specific world. It can be used to control physics engine, and another

world properties like ambient lighting. A model plugin is attached to a specific model and the user can

control robot’s joints and state. A sensor plugin is attached to one sensor and allows the user to acquire

information from the sensor and it also controls sensor properties. A system plugin is specified on the

command line and is the first to load during gazebo start up giving to the used the control over the start

up process.

In this work we will not use Gazebo plugins. In this work we resort solely to interaction between ROS

and Gazebo simulator. The relationship between this two structures will be explained next.

4.2.2 The best simulator for the iCub humanoid robot

[38] analyses which is the best simulator for the iCub humanoid robot. There is discussed the dynamics

simulation, since it is crucial for humanoid robotics. Gazebo is shown as the best simulator since it

supports multiple physics engines and can be easily extended with new features, thanks to its modular

and plugin-based structure, among other features. They did a report with some data from users of dif-

38

Table 4.1: Most important features for a simulator. Table from [38].

# Feature Rating1 Stability of Simulation Very Important, Crucial2 Speed Important3 Precision of Simulation Important4 Accuracy of Contact resolution Important

5 Same interface between realand simulated system Important

6 Computational load (CPU) Not So Important7 Computational load (memory) Not So Important8 Visual Rendering Not So Important

Table 4.2: Most important criteria for choosing a simulator. Table from [38].

# Most Important Criteria Users (%)1 Simulation very close to reality 32%2 Open-source 24%3 Same code for simulated and real robot 19%4 Light and Fast 11%5 Customization 6%6 No interpenetration between bodies 3%

ferent simulators. The results are presented in table 4.1 and 4.2, respectively about the most important

features in a simulator and the main criteria to take into account when choosing a simulator. There are

some other tables and data to select the best simulator dynamics. At the end, the chosen simulator was

Gazebo since it is the most used in humanoid robots’ simulation, it is open-source, it has an active com-

munity and supports multiple physics’ engine which makes it the ideal candidate to simulate dynamics

with contacts. More information about this study can be found in the article [38].

4.2.3 Gazebo and ROS

Since ROS is the most used robotic platform, it has already a package developed to use ROS and

Gazebo simulator together. Thus, ROS users can simulate their experiments and applications in a

simulated environment which also provides visualization tools. The interaction between these structures

is done by installing gazebo ros pkgs. With this package, when both applications are running, some

ROS topics are created so the user can access Gazebo information as link states, model state and

some parameters of Gazebo. It also creates some services of ROS to send control commands to

gazebo model and to extract the information from Gazebo. In figure 4.4, we can see a scheme with all

interaction between ROS, Gazebo and the plugin gazebo ros control that loads the hardware interfaces

of the robot.

Despite Gazebo being the best choice of simulator due to its dynamical system, to work with Gazebo

and ROS Indigo simultaneously, the adequate version of Gazebo to be used is 2.2 (released in 2013

November) and the latest Gazebo version is currently 6.1.0 released on 2015, August and in version

2.2, there are some features not available to use (like dynamics that has not enough stability). To do the

manipulation of the objects, a task with a certain level of complexity, without a good dynamic simulation

system, the best and safest option is to simulate static objects to make sure the results received are as

39

Figure 4.4: Gazebo an ROS architecture scheme (see text). Image from (http ://gazebosim.org/tutorials/?tut = ros control).

reliable as possible.

4.3 Hand Model for Gazebo

To create a model on Gazebo, we need to fulfill some requirements. There are some requirements for

both links and joints.

A link contains physical properties of the respective body of the model and it can have many collisions

and visual elements. The required element is the inertia which determines the dynamic properties. To

visualize the body we need to add a visual element to the link. If we will use collision information we also

need to add a collision element to the link and the sensor, in case we need sensor information. We can

represent links with meshes previously built or primitive shapes existing on gazebo database.

Joints connect links and have parented, child and origin elements as required. Parent and child

refer the parent link and the child link to connect with. The origin element means the position and the

orientation of the child link in the parent link reference frame.

Finally, we can add model plugin elements, in case we need some plugin to improve the control of

the model.

Figure 1.1 has the model of the hand used in this work. We used the meshes present to visualize

40

the iCub in rviz visualizer and some boxes to support the wrist of the hand model. We choose this way

to represent and support the hand because the process of loading all robot would be too expensive. So

we load just the essential parts of the robot (wrist, hand and fingers).

Figure 4.5: Minimum and maximum angles of the model hand joints (see text).

Initially we choose to work with a known model of a robotic hand, the hand from iCub. When we load

the iCub model of Gazebo, we notice that there was no hand on the robot so we had extract the hand

from an existing model for visualization on RViz. By adjusting some parameters and by adding other

required parameters for Gazebo, we got the hand presented in image 1.1.

In figure 4.5 we can see the palm and each link of each finger which turns from the position of the

figure 4.5a) until it reaches the limit of 90o according to the real model. The maximum reachable angles

of each joint are presented: the distal links at their maximum in sub figure 1.1b); the intermmediate and

distal links in 1.1c), proximal links, with exception of thumb, in 1.1d), thumb proximal reachable angle in

1.1e) and the abduction reachability of thumb in 1.1f) and 1.1g).

Now, we can send controls to the joints so they can bend and start to create some contacts.

4.4 Contact Computation

When sending commands to a certain joint, an effort is applied to the joint and it bends. With a set of

efforts to each joint of each finger we can create contacts. Figure 4.6 has an illustration of a contact: the

object is the white box. In 4.6a) there is the contact centered in the blue sphere and the normal of the

contact surface is in green. The indications in blue and green are shown when selected the option on

menu View - Contacts of Gazebo window. The sub figure 4.6b) is a zoom of 4.6a) and 4.6c) shows us

the other side of the object. There we can see the normal directions of the contact surface.

The contact model acquired from Gazebo is based on soft finger contact model since it returns a

contact structure composed by: all the positions(x, y and z coordinates), all normal of the applied forces

(x, y, z values, normalized so the vector norm be 1), the applied force (decomposed in x, y and z

coordinates), the torque about the normal of the contact and the name of the bodies in contact. The

contact information received from Gazebo is not stable. During simulation when there is a contact, the

finger that is touching the object is trembling due to the singularities associated to the contact and as

41

Figure 4.6: Index Finger touching an object (see text).

consequence the contact is not constant. When all the fingers are in contact with the object, the contact

information is received like showed in table 4.3.

The frequency of trembling is not the same for all contacts and then if you take a sample of contact

information, it has, for example, 2 fingers in contact, in the next sample there are no contacts, in the next

sample it has 3 contacts (one already detected in the first sample and 2 new contacts) and so on. To

a better understanding, in table 4.3, we show a set of samples from the topic. The topic does not give

us synchronized information within a unique sample but it has an advantage: the sampling frequency is

high and it not take us a long time to acquire all contacts.

This table was build when there is no contact yet and, at sample 15 is acquired the first contact from

the grasp. From that sample until the end of the table, we can see that the contacts will appear but they

are almost no sample that have all contacts detected. The values of the joints were not modified while

this acquisition was made.

We need to post-process that information so we can get all the contacts from the simulated grasp.

We acquire the information from all samples and ignore the not relevant contacts to the grasp, such as

the contact between the box from the base that supports the hand and the ground.

This acquisition is done using a temporal filtering since samples are not constant through time as

desired. This filtering allows us to acquire consistent information about all existing contacts in simulation.

Such adaptation to the simulator data is not that simple since, the interval of samples acquired is not

always the same. If we update the contact information based in just a sample at once, we would not

have all contact information existing in the environment and barely have a force closure grasp situation.

This limitation brings another problem: when we want to know if a contact does not exist anymore.

Just one acquisition is not enough to be sure about when a contact is removed. To take care of this we

create some thresholds to do that detection and update the real contact information.

To save data from contact information we copy the positions and normal data into vectors, one vector

of positions and one vector of normal to each link of each finger.

After we collect all contacts information, since the positions and normal received are in the reference

frame of the correspondent link, we transform all positions and normal in a common reference frame.

We are always verifying if any contact is no longer a contact in the simulation. We publish all the

information about the contacts in a topic named positions normals (see figure 4.3). In this topic we have

42

all information with just one sample. The choice of the reference frame is not relevant since the effect is

the same once the transforms are applied correctly.

With position and normal data of contacts already published in the topic, we are able now to calculate

if all the contacts create a force closure grasp. We do not save forces and torque from gazebo because

force closure test just needs the positions and normal and admits that any contact can be applied with

unlimited force to resist any unlimited external disturbance.

The contacts with any part of each finger is detectable since we have contact sensors in all links

surface of the hand, like we have in human hands. The palm will not be considered for the grasps.

# samples #contacts Bodies in collision’s names

0 0 -

1 1 box from base vs ground

2 0 -

3 0 -

4 0 -

5 1 box from base vs ground

6 0 -

7 0 -

8 0 -

9 0 -

10 0 -

11 0 -

12 1 box from base vs ground

13 1 box from base vs ground

14 0 -

15 2 object vs right index fingertip, box from base vs ground

16 1 box from base vs ground

17 4object vs right index fingertip, box from base vs ground, object vs right

ring fingertip, object vs right middle fingertip

18 1 object vs right index fingertip

19 2 object vs right middle fingertip, object vs right ring fingertip

20 3object vs right ring fingertip, object vs right middle fingertip, box from base

vs ground

21 2 object vs right ring fingertip, object vs right little fingertip

22 4object vs right index fingertip, box from base vs ground, object vs right

ring fingertip, object vs right middle fingertip

23 0 -

43

24 3object vs right index fingertip, object vs right ring fingertip, object vs right

middle fingertip

25 2 object vs right ring fingertip, object vs right little fingertip

26 5object vs right index fingertip, box from base vs ground, object vs right ring

fingertip, object vs right middle fingertip, object vs right thumb fingertip

27 1 object vs right little fingertip

28 5object vs right index fingertip, box from base vs ground, object vs right ring

fingertip, object vs right middle fingertip, object vs right thumb fingertip

29 1 object vs right little fingertip

30 5object vs right index fingertip, box from base vs ground, object vs right ring

fingertip, object vs right middle fingertip, object vs right thumb fingertip

31 1 object vs right little fingertip

32 4object vs right index fingertip, object vs right ring fingertip, object vs right

middle fingertip, object vs right thumb fingertip

33 3box from base vs ground, object vs right ring fingertip, object vs right

thumb fingertip

34 5object vs right index fingertip, box from base vs ground, object vs right ring

fingertip, object vs right middle fingertip, object vs right thumb fingertip

35 1 object vs right little fingertip

36 5object vs right index fingertip, box from base vs ground, object vs right ring

fingertip, object vs right middle fingertip, object vs right thumb fingertip

37 1 object vs right little fingertip

38 3box from base vs ground, object vs right ring fingertip, object vs right

thumb fingertip

39 1 object vs right little fingertip

40 6

object vs right index fingertip, box from base vs ground, object vs right

ring fingertip, object vs right middle fingertip, object vs right thumb

fingertip, object vs right little fingertip

41 0 -

42 5object vs right index fingertip, box from base vs ground, object vs right ring

fingertip, object vs right middle fingertip, object vs right thumb fingertip

43 2 box from base vs ground, object vs right ring fingertip

44 6

object vs right index fingertip, box from base vs ground, object vs right

ring fingertip, object vs right middle fingertip, object vs right thumb

fingertip, object vs right little fingertip

45 0 -

44

46 5object vs right index fingertip, box from base vs ground, object vs right ring

fingertip, object vs right middle fingertip, object vs right thumb fingertip

Table 4.3: Set of time samples from the topic we acquire contact information. Notice the inconsistencyof information during time without any motion of the fingers during the acquisition.

4.5 Force Closure Computation

With the points of contact established between the fingers and the object we will verify if the force closure

condition is satisfied. To do so, we will test the set of contacts received with the equations in 2.1.2. In

this test we considered each point of contact as a unique and compute the friction cone to each of them.

To compute the friction cone, we approximate the base by a polygon on x edges. The value of x is

named as the level of discretization of the friction cone.

Figure 4.7: Metric Results using different levels of discretization.

With different discretization levels of the friction cone, the result also change. We perform some

tests with the discretization level changing from 3 to 30. In figure 4.7, we have the resulting values from

one of those tests. To acquire this results, we compute the force closure test with one set of contacts

corresponding to a unique grasp pose. Looking at the graphic of figure 4.7, we can see the discretization

level is stable between the level 4 and 7 and then increases and start oscillating. The purpose is to have

a value as stable as possible so we will choose a level of discretization between 5 and 6. The time

interval took to compute the force closure result increases linearly so the best value is the smallest

possible. Once we also intend to use a stable value in a safe zone and smallest as possible. We use

the value 5 for the discretization level of the friction cone to calculate the force closure metric.

Instead of having one friction cone associated to each finger, we have as many friction cones as

45

contact points in each finger. This approach gives us better force closure results. This choice relies in

the fact that simulations does not give us constant data to process so we decide to consider all contact

points of each finger. This decision allows us to have better result because having just one point for

each contact, it could give us a false negative force closure result due to position and normal direction of

that point and with this we are sure that we are computing a more accurate result of force closure. This

is our variety of soft finger contact.

We adopted the work done in [1] and by adjusting some parameters, like discretization level from

friction cone, we achieve better results of force closure tests.

In terms of the implementation in ROS, since it is a method needed sporadically, we put the code into

a service node and once we need to know the result of the force closure test, we fill the request, create

a client and call the service by that client. The software architecture is in figure 4.3.

4.6 Motion Planning and MoveIt

4.6.1 Reaching the Object

To position each fingertip on the object we define a trajectory in each finger that results in a coordinated

hand closing motion without colliding to external objects or to other fingers. We have a well known

package from ROS, MoveIt which has the libraries OMPL and FCL previously introduced in chapter 2.

MoveIt is capable of choosing the best algorithms and methods to do motion planning with collision

avoidance according to available data about the environment. The information about the environment is

in a planning scene that has the robot and objects.

We need to deal with the fact that this package requires that the initial and final states need to be

in the object free area. One way to deal with this issue is to attach the object to the fingers allowing

collision between them but another issue appears, the fingers attached to the object are now able to

pass inside the object what is not feasible in the real world. Our solution, once we need collisions in the

final state, is based on the planning the trajectory to the closest collision free point from the object, and

then we increase the value of the joints’ degrees by a certain percentage so we get collision situation

with the object on the exact area we want the collision to happen. This trajectory is not planned with the

MoveIt algorithms.

To position each fingertip on the object we need to do the motion planning of each finger.

Since the finger gaiting motion is based on remove the contact and move the finger to the final

position, our approach starts by lift the finger from the object surface. To positioning the finger aligned

to the final pose we used the joints limits from each finger so the motion fit with any object. If the finger

can not pass in its own limits, so it can not pass.

To create the final contact between the finger and the object, we implement an algorithm. The

algorithm is an optimization to find the closest pose of the object, free of collision, based on a relation

with a geometric series:

46

20∑n=1

(1

2

)n

, (4.1)

and the resulting angle at n iterations is given by:

αn = αn−1 ± αmax ×(

1

2

)n

, α0 = 0. (4.2)

At each iteration of this algorithm, depending on the existence of collision, the finger keeps the motion

of getting closer from the object, in case of the previous iterations detects a collision, the finger moves

away from the object.

At the end, if there is any collision we subtract 0.1% of the maximum angle until it is a collision free

pose and then we do the motion planning. This percentage is low because we do not want the finger go

too far from the object.

For collision detection, we use the FCL, previously introduced in chapter 2.

With this solution we guarantee a collision free path and a final state with the desired collision be-

tween the object and the finger.

47

48

Chapter 5

Results

5.1 Problem Description

In this work it was developed a method to perform re-grasp actions using finger gaiting by identify which

fingers are expendable to a certain grasp so the finger gaiting can be made. To do that, we need to

know which fingers we can reposition on the object.

To have that information we need to know if it is safe to remove some fingers. Those fingers need

to be moved to another position. We use force closure tests to make sure that the grasps are always

forced closure grasps during the finger gaiting.

In this chapter we establish the baseline for the solution to the re-grasping problem with finger gaiting.

We will also show some choices made for some adjustable parameters.

5.2 Experimental Setup

To approach the re-grasping task using finger gaiting, we need a hand model, one object and, at least,

two different grasps to perform in-hand manipulation.

The hand model chosen is the model from the iCub robot, present in figure 1.1. The objects used

are illustrated in figure 5.1.

The re-grasp task consists in changing the contacts between some fingers and the object, to reach

a different grasp. We used the grasps present in figures 5.2 and 5.3.

Figure 5.1: Objects used in this algorithm.

49

Figure 5.2: The Version of Parallel extensiongrasp used in this work. Figure 5.3: Index Ring to Middle grasp (IR2M).

5.3 Reaching the First Grasp Pose

The first grasp is created by moving the fingers against the object like explained in 4.6.1. We imple-

mented a motion planning method to achieve the version of parallel extension grasp. With this grasp

pose we can show the robustness of the removable fingers identification since we could perform a force

closure grasp with just the middle and the thumb fingers but we use all fingers to show that some fingers

can be removed.

Initially, the hand model and the object are like in figure 5.4a). Next, we apply the motion planning

method and the fingers are placed on object surface like showed in figure 5.4b). All fingers are touching

the object.

Figure 5.4: a) The hand and the object in the simulation environment. b) First grasp pose.

5.4 In-hand Manipulation

In figure 3.1, we showed the flowchart scheme of this algorithm. Figures 5.5 and 5.6 shows us 4 different

ways of re-grasp, with the algorithm that leads to the same result using the box from figure 5.1.

During the test phase we mostly used the first sequence of the grasp phase, the upper sequence

from figure 5.5. It starts with the grasp pose from figure 5.4b), reposition the index and the ring fingers,

follow one of the sequences, reaching the grasp when all fingers are in contact with the object. From

there, it only needs to remove the little and the thumb fingers, by any order to reach the final pose. It can

50

Figure 5.5: 4 possible re-grasp sequences tested with the algorithm.

perform any sequence. The main advantage of performing any re-grasp sequence reflects in the case

of we need to remove, for example, index finger and it is not an expendable finger. Then, we can remove

ring finger instead of the opposite may also happen. With this, we have more chance of achieving the

final grasp pose. The algorithm gives preference to reposition the finger needed for the final grasp pose.

When they are repositioned, it remove the fingers no required for the final pose.

We show the robustness of the algorithm by using another object with a different shape: the cylinder

from figure 5.1. Performing this algorithm with an object like a cylinder is challenging because its surface

is not plane as a rectangle, which implies a more accurate re-grasp task to ensure force closure grasp

phases when re-grasping. Figure 5.6 illustrates the result of applying this approach with a different

object. It can perform one of the 4 possible sequences, present in figure 5.6 of the re-grasping task.

Notice that the movement of the index and ring fingers are the best to perform the re-grasp task

described since the fingers are shrunken at the maximum so they can pass behind any object. If the

fingers collide with the object, then the hand should be slightly moved away from the object so the fingers

can be repositioned.

5.5 Force Closure Computation

In addition to the unsynchronized information, we also have noisy contact normal and positions. This

issue increases the chances to get inconsistent data to compute the force closure test, corrupting the

51

Figure 5.6: 4 possible re-grasp sequences tested with the algorithm.

results of the test. To deal with this problem, we have two options: compute the mean of all contact

normal and positions from each link and compute the force closure with that mean value or, we use all

contact positions, and respective normal, to compute the force closure result.

We test both options, based on the sequence a, b1, c1, d, e1, f from figure 5.5. For each grasp

phase we acquire the set of contact normal and positions and compute the force closure test with the

mean calculation and without it.

In figure 5.7 we have the force closure results with and without the mean of the contact normal

and positions. This test was made using the box from image 5.1 as the object to grasp. For a better

understanding of the x axis:

• First letter correspond to the grasp phase in the sequence shown in figure 5.5:

– a - initial grasp pose

– b1 - changing the index finger

– c1 - index finger in the final position, ring finger in lifted

– d - index and ring fingers in the final positions

– e1 - little finger lifted

– f - both little and thumb fingers lifted; final grasp pose

52

• Second letter means the finger that we are testing with the test:

– i - index finger

– l - little finger

– m - middle finger

– r - ring finger

– t - thumb finger

If the metric value is negative, that means that the force closure result is False. If the metric value is

positive, that means that the force closure result is positive.

Figure 5.7: Metric Results using mean calculation, in blue, and using all contact points, in orange.

We notice that, without computing the mean, we get more accurate and credible results and while

the method is running, when using the metric with mean, sometimes the result is false. This happens

because we are computing the mean with the non constant data from the simulator. Without mean

calculations we also do not get constant points but the fact that we are using all the points, the metric

returns more constant results in terms of binary result so we use all points without averaging.

5.6 Limitations

We accomplished the proposed work, although, there is some issues to improve. We can start by

describing some limitations of this method.

With the present implementation of the contact acquisition, there is no ambition to wait for the ini-

tial grasp pose. This issue might bring some troubles, namely the fact that some required fingers to

reposition might not be available to reposition because they are not expendable from the grasp.

53

With another pair of grasps, this method is not able to verify if the sequence is feasible directly or

indirectly. It could be feasible but the order of gaiting might be crucial or it might be necessary resort to

another grasp pose that will connect the two sequences. The order of the gait can be done resorting to

the metric value from the force closure test result. For example, when two grasps can be reached used

on a sequence of re-grasp, we might choose the safer one.

54

Chapter 6

Conclusions

6.1 Achievements

This work proposed a methodology to perform in-hand re-grasping actions with finger gaiting. This

approach aimed, given an initial grasp pose, to changing the contacts between the fingers and the

object to reach a new grasp pose. The re-grasp task was performed always with a force closure grasp

so the object did not fall.

In this approach we did finger gaiting by moving one finger at a time to a final pose. This motion of

the fingers required an algorithm of motion planning, with collision avoidance for not creating undesired

collisions and perhaps, jeopardize the re-grasping process.

Moving one finger at a time required the identification of the fingers that could be removed from the

current set of contacts with the object. We identified the removable fingers by testing the stability of that

grasp without considering the finger under analysis. If the result of the force closure test remained true,

then the finger was removable from the grasp and we could move that finger freely to any other point of

contact on the object surface.

While performing the gaiting process, we removed and repositioned a finger at each time. As the

contacts changed, the force closure test returned the result related to the updated contact information.

We considered that any part of the finger can be in contact with the object, in other words, we

considered that all links from each finger have sensors on their surface allowing the acquisition of contact

information from any part of the finger.

We created a new unusual grasp pose that allowed us to do re-grasp with a static object in simulation,

IR2M - Index Ring to Middle.

This approach is different from those already developed since this one was developed in the ROS

robot platform, giving our contributions for the ROS community and also allows adaptability to other

robots. It is able to deal with sensors in all links in the fingers and it also works with sensors only on

fingertips.

We showed our way to do the motion planning for finger gaiting with collision avoidance and, at the

same time, create the desired contact situation with the object. We showed how to deal with gazebo

55

contact information using temporal filtering. We also showed that, with non-constant data acquisition,

the best way to compute force closure tests is using all contact points between the object and the fingers.

Also, we contributed for the community by introducing a model of the iCub hand to use in the Gazebo

simulator.

We showed the robustness level of this algorithm, not only in terms of variations of the object shapes,

but also in terms of performing different sequences of finger gaiting, reaching the same final grasp pose.

6.2 Future Work

Future work will improve this research, by enhance the repertoire of pre-defined grasp classes so it

could be used with this method. To enhance this suggestion it could be introduced grasp skills, not only

for the iCub but also to general dexterous hands

Developing a planner to ensure the final grasp pose is reachable from the initial one would be a

challenging approach. It could plan the all sequence of re-grasp and, if it needed, use sub-grasps to get

to the final grasp pose.

To transcend another limitation, we suggest to create some ambition to the algorithm so it wait until

the first grasp is reached and then perform the finger gaiting.

The validation of the compatibility with other robotic hands it could be enhance this work too.

We suggest the optimization the object surface area where each finger can reach.

Another way to keep this research is to couple the fingers joint as in the real iCub hand model,

favoring the use in the real robot. Notice that the current iCub hand model only has tactile sensor on

fingertips and on palm.

We suggest also to add rolling and sliding techniques to enhance re-grasp feature.

56

Bibliography

[1] F. Veiga. Robotic grasp optimization from contact force analysis. Master’s thesis, Instituto Superior

Tecnico, April 2012.

[2] I. A. Sucan, M. Moll, and L. E. Kavraki. Open motion planning library, December 2012.

[3] D. M. J. Pan, S. Chitta. FCL: A General Purpose Library for Collision and Proximity Queries,. in

IEEE Intl. Conf. on Robotics & Automation Magazine, Maio 2012.

[4] S. S. S. Richard M. Murray, Zexiang Li. A Mathematical Introduction to Robotic Manipulation. CRC

Press, 1994.

[5] J. Fischer, N. Thompson, and J. Harrison. The prehensile movements of the human hand. In

P. A. Banaszkiewicz and D. F. Kader, editors, Classic Papers in Orthopaedics, pages 343–345.

Springer London, 2014. ISBN 978-1-4471-5450-1. doi: 10.1007/978-1-4471-5451-8 85. URL

http://dx.doi.org/10.1007/978-1-4471-5451-8_85.

[6] M. Cutkosky and R. Howe. Human grasp choice and robotic grasp analysis. In S. Venkataraman

and T. Iberall, editors, Dextrous Robot Hands, pages 5–31. Springer New York, 1990. ISBN 978-1-

4613-8976-7. doi: 10.1007/978-1-4613-8974-3 1.

[7] J. B. I Kao, K. Lynch. Contact Modeling and Manipulation, pages 647–670. Springer, 2008.

[8] D. Prattichizzo and J. Trinkle.

[9] T. Karras and T. Aila. Fast parallel construction of high-quality bounding volume hierarchies. In

Proceedings of the 5th High-Performance Graphics Conference, HPG ’13, pages 89–99, New York,

NY, USA, 2013. ACM. ISBN 978-1-4503-2135-8. doi: 10.1145/2492045.2492055. URL http:

//doi.acm.org/10.1145/2492045.2492055.

[10] H. S. N. Walker, M. C. Lin. CAB: Fast Update of OBB Trees for Collision Detection Between Articu-

lated Bodies, volume 9(2). Journal of Graphical Tools, 2004.

[11] G. Zachmann. Rapid Collision Detection by Dynamically Aligned DOP-Trees. VRAIS ’98. IEEE

Computer Society, Washington, DC, USA, 1998. ISBN 0-8186-8362-7.

[12] X. Zhang and Y. Kim. k-IOS: Intersection of spheres for efficient proximity query. May 2012. doi:

10.1109/ICRA.2012.6224889.

57

[13] C. J. Ong and E. Gilbert. The gilbert-johnson-keerthi distance algorithm: a fast version for incremen-

tal motions. In Robotics and Automation, 1997. Proceedings., 1997 IEEE International Conference

on, volume 2, pages 1183–1189 vol.2, Apr 1997. doi: 10.1109/ROBOT.1997.614298.

[14] G. V. D. Bergen. Proximity queries and penetration depth computation on 3d game objects. In In

Game Developers Conference, volume 170, 2001.

[15] D. Tracy, S. Buss, and B. Woods. Efficient Large-Scale Sweep and Prune Methods with AABB

Insertion and Removal. March 2009. doi: 10.1109/VR.2009.4811022.

[16] M. Tang, Y. Kim, and D. Manocha. C2a: Controlled conservative advancement for continuous colli-

sion detection of polygonal models. In Robotics and Automation, 2009. ICRA ’09. IEEE International

Conference on, pages 849–854, May 2009. doi: 10.1109/ROBOT.2009.5152234.

[17] K. Abdel-Malek, J. Yang, D. Blackmore, and K. JOY. Swept volumes: fundation, perspectives, and

applications, volume 12. World Scientific, 2006.

[18] M. C. L. Ehmann, Stephen A. Accurate and fast proximity queries between polyhedra using convex

surface decomposition. Computer Graphics Forum, 20(3):500–511, 2001.

[19] T.-C. Publishing. Scene graphs (advanced methods in computer graphics) part 2, Dezem-

bro 2015. URL http://what-when-how.com/advanced-methods-in-computer-graphics/

scene-graphs-advanced-methods-in-computer-graphics-part-2/.

[20] I. N. G. Z. R. Weller, C. Mendoza. Kinetic Separation Lists for Continuous Collision Detection of De-

formable Objects. 3rd Workshop in Virtual Reality Interactions and Physical Simulation ”VRIPHYS”,

2006.

[21] I. Sucan and L. Kavraki. A sampling-based tree planner for systems with complex dynamics.

Robotics, IEEE Transactions on, 28(1):116–131, Feb 2012. ISSN 1552-3098. doi: 10.1109/TRO.

2011.2160466.

[22] I. Sucan and L. Kavraki. Kinodynamic Motion Planning by Interior-Exterior Cell Exploration, vol-

ume 57 of Springer Tracts in Advanced Robotics. Springer Berlin Heidelberg, 2010. ISBN 978-3-

642-00311-0. doi: 10.1007/978-3-642-00312-7 28.

[23] S. M. LaValle. Rapidly-Exploring Random Trees: A New Tool For Path Planning. 1998.

[24] Dijkstra’s algorithm, Dezembro 2014. URL http://en.wikipedia.org/wiki/Dijkstra%27s_

algorithm. Wikipedia.

[25] R. U. Kavraki Lab. The open motion planning library: A primer, 2014.

[26] K. Hsiao, S. Chitta, M. Ciocarlie, and E. G. Jones. Contact-reactive grasping of objects with par-

tial shape information. In Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International

Conference on, pages 1228–1235. IEEE, 2010.

58

[27] M. A. M. Henriques. Controlo e planeamento de maos roboticas antropomorficas utilizando siner-

gias. Master’s thesis, Instituto Superior Tecnico, Julho 2013.

[28] L. Han and J. C. Trinkle. Object reorientation with finger gaiting, April 1998.

[29] J. Hong, G. Lafferriere, B. Mishra, and X. Tan. Fine manipulation with multifinger hands. In Robotics

and Automation, 1990. Proceedings., 1990 IEEE International Conference on, pages 1568–1573

vol.3, May 1990. doi: 10.1109/ROBOT.1990.126232.

[30] J. Xu, T. Koo, and Z. Li. Finger gaits planning for multifingered manipulation. In Intelligent Robots

and Systems, 2007. IROS 2007. IEEE/RSJ International Conference on, pages 2932–2937, Oct

2007. doi: 10.1109/IROS.2007.4399189.

[31] J. Xu, T.-K. Koo, and Z. Li. Sampling-based finger gaits planning for multifingered robotic hand.

Autonomous Robots, 28(4):385–402, 2010. ISSN 0929-5593. doi: 10.1007/s10514-009-9164-5.

URL http://dx.doi.org/10.1007/s10514-009-9164-5.

[32] L. Han and J. Trinkle. Dextrous manipulation by rolling and finger gaiting. In Robotics and Automa-

tion, 1998. Proceedings. 1998 IEEE International Conference on, volume 1, pages 730–735 vol.1,

May 1998. doi: 10.1109/ROBOT.1998.677060.

[33] J.-P. Saut, A. Sahbani, and V. Perdereau. A Generic Motion Planner for Robot Multi-fingered Manip-

ulation. Advanced Robotics, 25(1-2):23–46, Jan. 2011. URL https://hal.archives-ouvertes.

fr/hal-00736011.

[34] Y. Bai and C. Liu. Dexterous manipulation using both palm and fingers. In Robotics and Automation

(ICRA), 2014 IEEE International Conference on, pages 1560–1565, May 2014. doi: 10.1109/ICRA.

2014.6907059.

[35] T. P. Attawith Sudsang. Regrasp planning for a 4-fingered hand manipulating a polygon, September

2003.

[36] M. Quigley, K. Conley, B. P. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng. Ros: an

open-source robot operating system. In ICRA Workshop on Open Source Software, volume 3, May

2009.

[37] N. Koenig and A. Howard. Design and use paradigms for gazebo, an open-source multi-robot

simulator. In IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 2149–

2154, Sendai, Japan, Sep 2004.

[38] S. Ivaldi, J. Peters, V. Padois, and F. Nori. Tools for simulating humanoid robot dynamics: a survey

based on user feedback. In Proceedings of the IEEE-RAS International Conference on Humanoid

Robots (Humanoids), pages 842 – 849. Madrid, Spain, Nov 2014.

59

60

Appendix A - TF Frame Trees

61

view_fram

es Result

root_link

base_box

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

r_hand

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

base_box1

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

base_link

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

box_model

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

base_box2

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

base_box3

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

world

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

r_indexbase

r_index0

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

r_index1

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_littlebase

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_middlebase B

roadcaster: /robot_state_publisherA

verage rate: 10.227 Hz

Most recent transform

: 67.800 ( 0.045 sec old)B

uffer length: 4.400 sec

r_ringbase

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_thumb0

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_little0

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

r_little1

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_middle0

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

r_middle1

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_ring0

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

r_ring1

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_thumb1

Broadcaster: /robot_state_publisher

Average rate: 50.223 H

zM

ost recent transform: 68.292 ( -0.447 sec old)

Buffer length: 4.480 sec

r_thumb2

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_index2

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_index3

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_little2

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_little3

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_middle2

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_middle3

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_ring2

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_ring3

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_thumb3

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

r_thumb4

Broadcaster: /robot_state_publisher

Average rate: 10.227 H

zM

ost recent transform: 67.800 ( 0.045 sec old)

Buffer length: 4.400 sec

Recorded at tim

e: 67.845

62