# Multi-fingered robotic hand planner for object reconfiguration through a rolling contact evolution model

Juan Antonio Corrales Ramon, Veronique Perdereau, Fernando Torres Medina

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

## To cite this version:

Juan Antonio Corrales Ramon, Veronique Perdereau, Fernando Torres Medina. Multi-fingered robotic hand planner for object reconfiguration through a rolling contact evolution model. 2013 IEEE International Conference on Robotics and Automation (ICRA), May 2013, Karlsruhe, Germany. pp.625-630, 10.1109/ICRA.2013.6630638. [hal-03615406](https://hal.science/hal-03615406v1)

HAL Id: hal-03615406

https://hal.science/hal-03615406v1

Submitted on 13 Dec 2025

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

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

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

Distributed under a Creative Commons Attribution 4.0 International License
# Multi-fingered Robotic Hand Planner for Object Reconfiguration through a Rolling Contact Evolution Model

Juan Antonio Corrales Ramón, Véronique Perdereau and Fernando Torres Medina

**Abstract**— This paper presents a novel planner for dexterous manipulation with a multi-fingered robotic hand. This planner receives as input an initial grasp of the object and a desired trajectory of the object in task space (position and orientation). The planner computes the movements of the fingers which are required to move the object along this trajectory without breaking contacts. It uses a triangle mesh representation of the surfaces of the fingers and the object and a new contact evolution graph in order to compute all the possible transitions between the contact primitives of their surfaces. This planner have been implemented as a program which communicates with a five-fingered hand in order to test them in real manipulation tasks.

## I. INTRODUCTION

Multi-fingered robotic hands are becoming more and more widespread in robotic workspaces because of their flexibility with regard to traditional parallel grippers. They are able not only to perform grasping tasks of objects with complex shapes but also to complete dexterous manipulation tasks. Dexterous manipulation is a type of robotic task where a multi-fingered hand or a group of cooperating manipulators perform a sequence of movements in contact with an object in order to achieve a goal configuration (position and orientation) for the object and for the contact points [1], [2]. Thereby, dexterous manipulation can be stated as a planning problem of the hand fingers movements whose goal is the evolution of a grasped object from an initial configuration to a final desired one. This is a complex problem since it involves taking into consideration an important number of constraints which restrict the number of possible solutions for the problem. Two of the most important constraints which have to be considered in this problem are: contact maintenance and finger kinematics. On one hand, the contact maintenance constraints guarantee that the fingers which touch the object do not break contact and thus the stability of the object is kept during the manipulation. On the other hand, the finger kinematic constraints guarantee that the kinematic chain that composes each finger is able to reach the desired contact point over the object. Three main techniques have been used in previous research for solving this dexterous manipulation problem: control schemes for hybrid systems,

probabilistic motion planning techniques and graph search techniques.

The dexterous manipulation planning problem can be represented as a control theory problem where a control law makes the system evolve from an initial state (initial grasp) to a final state (desired configuration of the object). Due to the combination of continuous (e.g. continuous trajectory of the object) and discrete events (e.g. contact mode changes and finger gaiting), dexterous manipulation planners based on control theory use hybrid representations, such as stratified systems and hybrid automata. Stratified systems [3], [4] and [5] represent the configuration space of the manipulation task as a hierarchy of strata where each stratum corresponds to a relative contact configuration of a finger or several fingers with the object. The planner combines the motion equations of every stratum and transitions between them in order to solve the manipulation problem. Manipulation planners [6] and [7] are based on hybrid automata which are composed by continuous states (i.e. positions and orientations of the object and the fingers) and discrete states (i.e. continuous manipulation mode and finger substitution mode). The dexterous manipulation planning is restated as the problem of driving the automaton from an initial state towards a final state through a sequence of transitions between continuous and discrete states decided by a switching controller.

Probabilistic motion planners based on RRT (Rapidly-exploring Random Tree) [8] have also been applied in dexterous manipulation tasks. This probabilistic technique is based on the random construction of a tree whose nodes represent free configurations of the robot system (i.e. not colliding with obstacles) and whose edges represent transitions between these nodes which observe the non-holonomic constraints of the planning problem. In [9] a dexterous manipulation planner divided into two levels (global planner and local planner) is proposed. The global planner constructs a RRT of the object configuration space in order to arrive to the goal configuration while the local planner checks whether the trajectories between two nodes of the RRT are feasible by solving the inverse dynamic problem for all contact combinations [10]. Another probabilistic method is developed in [11] and [12], which improves the computational efficiency of the previous planner and includes finger gaiting.

Graph search techniques are another way of solving the dexterous manipulation planning problem. Two different graph representations have been applied in these techniques. In the first graph representation, each node of the graph is equivalent to a specific contact relation between the fingers and the object. For instance, a dexterous manipulation algorithm is presented in [13] where two trees of contact formations (i.e. pairs of contact primitives - edges and vertices - between the surfaces of the fingers and the object) are grown from the initial and final grasps until they reach a common node. A similar approach is developed in [14] by

<footnote>The research leading to these results has been supported by the
HANDLE project which has received funding from the European
Community's Seventh Framework Program (FP7/2007-2013) under Grant
Agreement no. 231640 and by the research projects DPI2011-22766 and
DPI2012-32390 of the Spanish MEC.</footnote>

<footnote>J. A. Corrales is with the Institut des Systèmes Intelligents et de Robotique, Univ. Pierre et Marie Curie - Paris 6, 4 place Jussieu 75252 Paris cedex 05, France (phone: +33 (0) 1 44 27 96 29; fax: +33 (0) 1 44 27 51 45; e-mail: corrales_ramon@isir.upmc.fr).</footnote>

<footnote>V. Perdereau is with the Institut des Systèmes Intelligents et de Robotique, Univ. Pierre et Marie Curie - Paris 6, 4 place Jussieu 75252 Paris cedex 05, France (e-mail: veronique.perdereau@upmc.fr).</footnote>

<footnote>F. Torres is with Physics, Systems Engineering and Signal Theory Department, University of Alicante, Carretera de San Vicente del Raspeig, s/n, San Vicente del Raspeig (Alicante), Spain (e-mail: fernando.torres@ua.es).</footnote>
extending the contact formations concept to canonical grasps and defining a grasp transformation graph between them. In the second graph representation, each node of the graph is a configuration of the object without taking into consideration the hand information. In [15] and [16] a two-level (global and local) planner based on this graph representation is developed. The global planner organizes the object configuration space in a grid where an A\* search algorithm is executed in order to find a sequence of grid cells that connects the initial and final configurations of the object. The local planner verifies if each pair of grid cells can be joined by a continuous motion of the hand-object system.

These approaches solve the dexterous manipulation planning problem but all of them establish some restrictions about the surfaces of the object and the fingers. Some of them (such as [7], [13], [14]) are defined only for planar objects. Other ones ([9], [10], [15]) can only be applied to fingers and objects with parametric surfaces (such as ellipsoids, spheres, b-splines and cylinders) since they use Montana's equations [17] or their own parametric equations for contact evolution computation. Others use sharp fingertips in order to simplify their shape to a unique contact point. This paper presents a novel dexterous manipulation planner which uses a general representation of the surfaces of the fingers and the object so that it can be applied to any type of convex shape. Both the surfaces of the object and the fingers are represented as triangle meshes (see Fig. 1). This representation permits interoperability with other software and its level of detail can be adapted by changing the number of triangles of the meshes. The proposed planner solves the contact maintenance and the finger kinematic constraints of the manipulation planning problem with this surface representation in order to develop more general manipulation tasks.

<figure><img src="image_3.png" /><figcaption>Figure 1. Triangle mesh representation of the hand and an object.</figcaption></figure>

## II. DESCRIPTION OF THE MULTI-FINGERED HAND PLANNER

The proposed planner uses the geometric model of the surfaces of the object and the fingers described above in order to determine the evolution of the contacts between the fingers of the robotic hand and the manipulated object. The planner receives as input an initial grasp of the object and an trajectory of the object in task space. From these input parameters, the planner has to compute the changes of the fingers joint angles which drive the object from the initial grasp towards the final desired configuration along the desired trajectory.

Normally, the desired trajectory matches a desired task which should be performed by the hand over the object. For instance, turning it in order to give it to a human operator or translating it in order to insert it into a desired place which requires a specific configuration of the object due to collision constraints. The initial grasp is not computed by this

algorithm and any previous grasp synthesis algorithm based on quality measures could be used [18].

The object trajectory is composed by a sequence of desired object configurations in task space from the initial grasp. The local planner is responsible for computing the movements of the fingers which have to be applied in order to move the object from the current configuration to each intermediate configuration of the object trajectory. In order to do this, it computes the evolution of the contact points between the surfaces of the object and the fingers. In order to do this, it considers the possible rolling movements which can happen between the different primitives (vertices, edges and faces) which constitute their mesh surfaces.

The planner can be divided into three main phases: object-fingers contact configuration computation, generation of possible new contact configurations and verification/execution of the feasible finger movements. The pseudo-code of the implementation of these phases is shown in Table I and it is explained in detail throughout the following subsections.

<table><caption>TABLE I. IMPLEMENTATION OF THE PLANNER</caption><thead><tr><th colspan="2">Multi-fingered Hand Planner Algorithm</th></tr></thead><tbody><tr><td colspan="2"><strong>Input:</strong> <sup>W</sup><sub>M<sub>O</sub></sub>|<sub>T</sub>|, meshes of the fingers <i>M</i><sub>F</sub>, and the object <i>M</i><sub>O</sub>.</td></tr><tr><td colspan="2"><strong>Implementation:</strong></td></tr><tr><td colspan="2">01: Obtain contacts between object and fingers on tactile sensors.</td></tr><tr><td colspan="2">02: Search the faces of the meshes which are the nearest to the contacts.</td></tr><tr><td colspan="2">03: Obtain contact states from contact points: <i>ListI</i> = [(<i>c</i><sub>O</sub><sup>j</sup>, <i>c</i><sub>F</sub><sup>j</sup>)]<sup>∀i</sup></td></tr><tr><td colspan="2">04: for each finger <i>F</i><sub>i</sub> in contact</td></tr><tr><td>05:</td><td><i>ListC</i> = Ø // Initialize the list of contact states</td></tr><tr><td>06:</td><td><i>incr_Q</i> = MAX_FLOAT // Initialize sum of increments</td></tr><tr><td>07:</td><td><i>Δq</i><sub>F</sub> = Ø // Initialize vector of joint increments</td></tr><tr><td>08: for each contact state <i>j</i> with contact primitives (<i>c</i><sub>O</sub><sup>j</sup>, <i>c</i><sub>F</sub><sup>j</sup>) in <i>ListI</i></td><td></td></tr><tr><td>09: Update coordinates <sup>W</sup><sub>c<sub>c<sub>O</sub></sub></sub>|<sub>t</sub>| of object primitive with (1).</td><td></td></tr><tr><td>10: Update coordinates <sup>W</sup><sub>c<sub>c<sub>F</sub></sub></sub>|<sub>t</sub>| of finger primitive with (2).</td><td></td></tr><tr><td>11: <i>ListC</i> = <i>ListC</i> ∪ [(<i>c</i><sub>O</sub><sup>j</sup>, <i>c</i><sub>F</sub><sup>j</sup>), <sup>W</sup><sub>c<sub>c<sub>O</sub></sub></sub>, <sup>W</sup><sub>c<sub>c<sub>F</sub></sub></sub>, <sup>M<sub>O</sub></sup><sub>c<sub>c<sub>O</sub></sub></sub>, <sup>M<sub>F</sub></sup><sub>c<sub>c<sub>F</sub></sub></sub>, <sup>c<sub>c<sub>O</sub></sub></sup><sub>c<sub>c<sub>F</sub></sub></sub>]</td><td></td></tr><tr><td>12: for each transition <i>k</i> of contact graph from state <i>j</i> to state <i>kj</i></td><td></td></tr><tr><td>13: if <i>c</i><sub>O</sub><sup>j</sup> ≠ <i>c</i><sub>O</sub><sup>kj</sup></td><td></td></tr><tr><td>14: Obtain the new coordinates <sup>W</sup><sub>c<sub>c<sub>O</sub></sub></sub>|<sub>t</sub>| with (1).</td><td></td></tr><tr><td>15: end if</td><td></td></tr><tr><td>16: Update <sup>W</sup><sub>c<sub>c<sub>F</sub></sub></sub>|<sub>t</sub>| by applying rolling (Sect. III.B).</td><td></td></tr><tr><td>17: <i>ListC</i> = <i>ListC</i> ∪ [(<i>c</i><sub>O</sub><sup>kj</sup>, <i>c</i><sub>F</sub><sup>kj</sup>), <sup>W</sup><sub>c<sub>c<sub>O</sub></sub></sub>, <sup>W</sup><sub>c<sub>c<sub>F</sub></sub></sub>, <sup>M<sub>O</sub></sup><sub>c<sub>c<sub>O</sub></sub></sub>, <sup>M<sub>F</sub></sup><sub>c<sub>c<sub>F</sub></sub></sub>, <sup>c<sub>c<sub>O</sub></sub></sup><sub>c<sub>c<sub>F</sub></sub></sub>]</td><td></td></tr><tr><td>18: end for</td><td></td></tr><tr><td>19: for each contact state <i>kj</i> in <i>ListC</i></td><td></td></tr><tr><td>20: Calculate position of fingertip by rolling of contact <i>kj</i>.</td><td></td></tr><tr><td>21: Obtain finger joints <i>Δq</i><sub>kj</sub> from Jacobian (Sect. II.C.3).</td><td></td></tr><tr><td>22: if <i>Δq</i><sub>kj</sub> is feasible</td><td></td></tr><tr><td>23: if <i>incr_Q</i> &gt; &sum; |<i>Δq</i><sub>kj</sub>|[<i>r</i>] // Where <i>r</i> is a joint of <i>F</i><sub>i</sub></td><td></td></tr><tr><td>24: <i>incr_Q</i> = &sum; |<i>Δq</i><sub>kj</sub>|[<i>r</i>]</td><td></td></tr><tr><td>25: <i>Δq</i><sub>F</sub> = <i>Δq</i><sub>kj</sub> // Choose smallest joint variation.</td><td></td></tr><tr><td>26: end if</td><td></td></tr><tr><td>27: end if</td><td></td></tr><tr><td>28: end for</td><td></td></tr><tr><td>29: end for</td><td></td></tr><tr><td>30: if <i>Δq</i><sub>F</sub> = Ø</td><td></td></tr><tr><td>31: return // No possible movement for <i>F</i><sub>i</sub>.</td><td></td></tr><tr><td>32: end if</td><td></td></tr><tr><td>33: end for</td><td></td></tr><tr><td>34: Execute <i>Δq</i><sub>F</sub>, of all contact fingers <i>F</i><sub>i</sub> of the hand.</td><td></td></tr><tr><td>35: <strong>Goto</strong> 01 for next object configuration <sup>W</sup><sub>T,M<sub>O</sub></sub>|<sub>T</sub>| in trajectory.</td><td></td></tr></tbody></table>
### A. Object-Fingers Contact Configuration Computation

First of all, the local planner has to determine the current relative contact configuration between the object and the fingers. Since the planner has been implemented as a C++ program which can communicate with a simulator of the hand [19] and with the real hand, the contact configuration can be obtained in two different ways. When the planner communicates with the simulator, the collision detection functions [20] of the simulator are used to obtain the contact points between the triangle mesh of the object and the triangle meshes of each finger. When the planner is connected to the real hand, the contact points are obtained from the tactile sensors installed at the fingertips of the hand. In both cases, a set of contact points and their corresponding coordinates are obtained (line 1 of Table I). Next, it determines which pair of faces of the object and finger surfaces correspond to each contact point by looking for the closest triangles in their triangle meshes (line 2 of Table I). The relative contact configuration between each pair of triangles is determined by computing if they have a pair of touching primitives (face, vertex or edge) [21]. The touching primitives are used to represent the initial contact configuration.

Thereby, after this process, the planner has an initial contact state for each contact point detected. Each contact state $j$ is characterized by a pair $(c_O^j, c_{F_i}^j)$ of primitives of the object $O$ and the finger $F_i$ which are touching, their global coordinates $(^W p_{c'_O} (t-1), ^W p_{c'_F} (t-1))$ in the world frame $W$ at step t-1 and their local coordinates $(^{M_O} p_{c'_O}, ^{M_F} p_{c'_F})$ in the frames of the corresponding triangle meshes $M_O$ and $M_{F_i}$ (line 3 of Table I). The global coordinates of the primitives are used to determine the possible relative rolling movements between their surfaces while the local coordinates are used to determine which neighbor primitives will come into contact after these rolling movements.

### B. Generation of Possible New Contact Configurations

After having calculated the current contact configuration between the object and the fingers, the planner determines how each pair of contact primitives might evolve when the object is situated in the next configuration of the trajectory. This configuration is represented by the transformation matrix $^W T_{M_O} |t|$ between frames $M_O$ and $W$.

Firstly, the planner supposes that the object is at the next configuration $t$ of the trajectory and also considers that the relative contact configuration between the object and the finger has not changed. In other words, the contact primitives are the same but their global coordinates have changed. The new coordinates $^W p_{c'_O} |t|$ of each contact primitive $c_O^j$ of the object (line 9 of Table I) are obtained by applying the transformation matrix $^W T_{M_O} |t|$:

$$ ^W p_{c'_O} |t\rangle = ^W T_{M_O} |t\rangle \cdot ^{M_O} p_{c'_O} \quad (1) $$

The coordinates $^W p_{c'_F} |t|$ of each contact primitive $c_F^j$ of finger $F_j$ (line 10 of Table I) are obtained by taking into account that its coordinates $^{M_O} p_{c'_F} |t|$ in the local frame

$M_O$ of the object are the same as in the previous step t-1 since the relative contact configuration remains equal:

$$ ^W p_{c'_F} |t\rangle = ^W T_{M_O} |t\rangle \cdot ^W T_{M_O} |t-1\rangle^{-1} \cdot ^W p_{c'_F} |t-1\rangle \quad (2) $$

Therefore, the planner obtains a new contact state at step $t$ for each previous contact state at step t-1. Although these new contact states involve that the contact primitives are the same in steps t-1 and $t$, there could also be a relative movement between the surfaces of the object and the fingers so that new primitives come into contact. For this reason, the local planner also computes these new possible contact primitives provoked by the rolling between the surfaces in contact (lines 12-18 of Table I). This contact evolution has been represented by a contact evolution graph, which will be described in detail in Section III. Thereby, a list of possible new contact states at step $t$ is generated from all the contact states of each finger at step t-1 (variable ListC in Table I).

### C. Verification/Execution of feasible finger movements

Although the previous step of the planner identifies a list of possible contact configurations between the object and the finger, not all of them can be performed since the fingers might not be able to reach the new contact points because of their kinematic limits. Therefore, each element of the previous list of possible contact states should be verified by applying the pseudo-inverse of the Jacobian $J^+$ of the corresponding finger to the displacement $\Delta p$ of the contact primitives from the initial contact state to the current possible contact state of the list. However, since the contact primitives may change during each contact transition due to rolling, the Jacobian should be recomputed at each new contact point. In order to avoid this inefficient procedure, the Jacobian is always computed at a common point (the endpoint of the fingertip) and the displacements $\Delta p$ are also computed from this point. Thereby, in each contact state, not only the rolling transformations are applied to the contact primitives but also to the fingertip's endpoint (line 20 of Table I).

The product between $J^+$ and $\Delta p$ yields the required increment $\Delta q$ of the joint values of the finger in order to move the finger to the desired contact position (line 21 of Table I). This product is repeated iteratively until the position error (difference between the real fingertip position obtained by forward kinematics and the desired one) is below a pre-established threshold. If during this process, the pseudo-inverse cannot be computed or the obtained joint rates are too high, the contact point is not reachable by the finger and the corresponding contact state is not considered (line 22 of Table I). Among all the resulting contact states which are kinematically feasible, the planner chooses the contact state which requires a minimum variation of the joint values (lines 23-26 of Table I). Finally, the planner moves the fingers by applying the computed new joint values (line 34 of Table I) and then the planner considers the next object configuration in the desired object trajectory (line 35 of Table I). This procedure is repeated until the object arrives at the final desired configuration of the trajectory or when the list of possible new contact states for a finger is empty because neither of them is kinematically feasible (lines 30-32 of Table I).
### III. CONTACT EVOLUTION BETWEEN SURFACE PRIMITIVES

#### A. Contact Evolution Graph

As stated in the previous section, the computation of new contact states generated by the rolling between the surfaces of the object and the fingers is obtained by applying a contact evolution graph, which is shown in Fig.2. On one hand, each node of this graph models a contact state between one primitive of the object ($f_O$; $e_O$; edge; $v_O$; vertex) and one primitive of one finger ($f_{F_i}$; face; $e_{F_i}$; edge; $v_{F_i}$; vertex). Only non-degenerate pairs [22] of contact primitives have been considered since contact between vertices and edges are not stable. On the other hand, each edge of the contact graph represents a transition between two consecutive contact states.

The initial contact state in the contact evolution graph at each step $t$ is the contact state computed by the local planner with expressions (1) and (2) (lines 9-11 of Table I). This contact state represents the new possible contact configuration if no rolling has taken place. Then, the local planner generates a list of new contact states which could be created because of the rolling or rotation between the primitives of this initial state. This rotation ends when one of the primitives collides with a neighbor primitive of the initial state and thus a new contact state is created. For instance, if the initial contact state is composed by the pair ($f_O, v_{F_i}$), the following contact states could be generated by the rotation of the finger surface around the vertex $v_{F_i}$ until an adjacent edge $e_{F_i}$ (i.e. new contact state ($f_O, e_{F_i}$)) or an adjacent face $f_{F_i}$ (i.e. new contact state ($f_O, f_{F_i}$)) touches the object face $f_O$. The planner does not only generate one contact transition for each initial contact state, but it generates all the possible contact transitions with every neighbor primitive (lines 12-18 of Table I). In the previous example, the planner obtains a list of the edges and faces which contain the vertex $v_{F_i}$, creates all the new possible contact states for all of them and computes the coordinates of their primitives by supposing rolling transitions. If the new contact state has a new object primitive, its global coordinates are obtained by applying (1) (lines 13-15 of Table I) since the object is supposed to be at the desired configuration. The coordinates of the finger primitive of the new contact state are obtained by supposing the rotation of the initial finger primitive around the initial object primitive (line 16 of Table I). The following subsection describes in detail how these rotations of the finger primitives are computed by the planner.

<figure><img src="image_4.png" /><figcaption>Figure 2. Contact evolution graph for new contact states due to rolling.</figcaption></figure>

#### B. Contact Transitions Generated by Rolling

As described above, the edges of the contact graph which connect two different nodes represent a contact transition (i.e. a change from an initial contact state towards a new final contact state). This transition is generated by the rolling between the two contact primitives of the initial contact state until one of them is substituted by the neighbor primitive of the final contact state. The contact graph contains 14 contact transitions. Two representative transitions will be described in detail since the other ones are obtained in a similar way. In particular, the edge from state ($e_O, f_{F_i}$) to state ($f_O, f_{F_i}$) and the edge from state ($f_O, v_{F_i}$) to state ($f_O, f_{F_i}$) are detailed next.

The transition from state ($e_O, f_{F_i}$) to state ($f_O, f_{F_i}$) involves that one face $f_{F_i}$ (i.e. triangle) of finger $F_i$ is initially touching the object through an edge $e_O$ but the finger face $f_{F_i}$ rotates around this edge until it collides with an adjacent face $f_O$ of the object (see Fig. 3). The global coordinates of the vertices of face $f_O$ do not change with respect to the initial contact state since it is the finger which rotates around the object surface and they are obtained by applying equation (1) (line 14 of Table I). Afterwards, the local planner has to calculate the new global coordinates of the vertices of the finger face $f_{F_i}$ (line 16 of Table I).

<figure><img src="image_5.png" /><figcaption>Figure 3. Transition from state ($e_O, f_{F_i}$) to state ($f_O, f_{F_i}$).</figcaption></figure>

Firstly, the local planner computes the angle $\alpha_{FIO}$ which should be rotated by face $f_{F_i}$ around edge $e_O$ in order to make $f_O$ and $f_{F_i}$ lay in the same plane. This angle is equal to the supplementary angle between the normal vectors $n_O$ and $n_{F_i}$ of both faces since they should have opposite directions:

$$ \alpha_{F,O} = \pi - \arccos(n_{F_i} \cdot n_O) \quad (3) $$

Next, the local planner has to use the Rodrigues' formula in order apply a rotation of $\alpha_{FIO}$ around the normalized axis $\hat{e}_O$ to the three vertices of the finger face ($f_{F_i}^{v_0}, f_{F_i}^{v_1}, f_{F_i}^{v_2}$). Nevertheless, since the rotation axis $e_O$ may not go through the origin, each vertex $j$ of the finger face $f_{F_i}^{v_1}$ should be translated by the first vertex of the axis $e_O^{v_0}$ in order to guarantee that the rotation is applied around the origin. Afterwards, this translation should be undone in order to place the vertex at its original location. The following expression summarizes this procedure:

$$ \begin{aligned} f_{F_i}^{v_1} = & (f_{F_i}^{v_0} - e_O^{v_0}) \cos \alpha_{F_i, O} \\ & + [\hat{e}_O \times (f_{F_i}^{v_1} - e_O^{v_1})] \cdot \sin \alpha_{F_i, O} \\ & + \hat{e}_O [\hat{e}_O \cdot (f_{F_i}^{v_1} - e_O^{v_1})] (1 - \cos \alpha_{F_i, O}) + e_O^{v_1} \end{aligned} \quad (4) $$
Finally, the local planner also applies this rotation transformation to the fingertip position in order to calculate its new location (line 20 of Table I) and multiply it by the pseudo-inverse of the Jacobian matrix (line 21 of Table I), as described in Section II.C.

The transition from state $(f_O, v_{F_i})$ to state $(f_O, f_{F_i})$ involves that the finger rotates around the vertex $v_{F_i}$ until de adjacent face $f_{F_i}$ collides with the object face $f_O$ (see Fig. 4). As in the previous transition, the global coordinates of the face $f_O$ do not change but the local planner has to compute the new global coordinates of the vertices of the finger face $f_{F_i}$.

<figure><img src="image_6.png" /><figcaption>Figure 4. Transition from state $(f_O, v_{F_i})$ to state $(f_O, f_{F_i})$.</figcaption></figure>

In this case, the axis of rotation $\hat{a}$ is the cross product between the normal vectors and the angle of rotation is also computed as the supplementary angle between these normal vectors, as in (3). Finally, the Rodrigues formula is used but a translation by vertex $v_{F_i}$ is applied in order to perform rotation at the origin, as shown below:

$$ 
\begin{aligned}
f'_{F_i} = & (f_{F_i}^v - v_{F_i}) \cos(\alpha_{F_i, O}) \\
& + [\hat{a} \times (f'_{F_i} - v_{F_i})] \cdot \sin(\alpha_{F_i, O}) \\
& + \hat{a} \cdot (f_{F_i}^v - v_{F_i}) [1 - \cos(\alpha_{F_i, O})] + v_{F_i}
\end{aligned} 
\quad (5) 
$$

## IV. EXPERIMENTAL RESULTS

The dexterous manipulation planner described in the previous sections has been implemented as a C++ program which can communicate with a robotic simulator of the hand [19] or with the real hand: a five-fingered Shadow Hand with 20 degrees-of-freedom (which mimics the kinematic structure of the human hand) and with tactile sensors over its fingertips (which are able to obtain the coordinates of the contact points and the corresponding contact 6D forces/torques). The implemented program receives as input the desired object trajectory. Each step of the hand trajectory computed by the planner is sent to the robot (simulation or real hand). The controller of the hand implements an hybrid position-force controller which guarantees not only that the hand reaches the commanded joint angles but also that their fingertips apply a minimum contact force which avoids undesired slippage.

Two different manipulation tasks of two different objects have been performed in order to verify the correct behavior of the proposed algorithm. The first manipulation task involves rotating a coke can by 20° around its longitudinal axis with movements of the thumb and the first finger of the hand. Fig. 5a and Fig 5b show these configurations during the execution of the task with the real hand. A marker is installed at the base of the can in order to compare the initial and final

orientations of the can and verify that a 20° rotation has been achieved. The object trajectory was composed by 4 elements.

<figure><img src="image_7.png" /><figcaption>Figure 5. Execution of a rotation task of a coke can: (a) initial and (b) final configuration of the system object-hand.</figcaption></figure>

The second manipulation task involves translating a cube along 15mm towards the palm of the hand with movements of the thumb and the first finger of the hand. The initial and final configurations of the hand and the object are shown in Fig. 6a and Fig. 6b. A tape measure is added in order to verify that a displacement of 15mm has been performed. The object trajectory was composed by 4 elements.

<figure><img src="image_8.png" /><figcaption>Figure 6. Execution of a translation task of a cube: (a) initial and (b) final configuration of the system object-hand.</figcaption></figure>

Fig. 7a. shows the angles which have been executed by the joints of the fingers (TH -thumb- and FF -first finger-) during the translation of the cube. The 4 steps of the trajectory can be easily identified by increments and decrements of joint angles. Fig. 7b. depicts the evolution of the normal contact force which is measured by the tactile sensor installed at the fingertip of the first finger. The hybrid position-force controller which executes the joint commands of the planner guarantees a minimum contact force of -0.8N in order to avoid contact breaking. In this plot, the 4 steps of the object trajectory can be also identified by a decrement of the contact force due to the movements of the fingers which is quickly corrected by the controller.

<figure><img src="image_9.png" /><figcaption>Figure 7. (a) Evolution of finger joint angles (left) and (b) evolution of normal contact force during the translation task of a cube.</figcaption></figure>
Since the proposed planner has to generate new contact states from the neighbor primitives (i.e. vertices, edges and faces) of the surfaces of the object and the fingers, its computational cost will depend on the number of these primitives. In order to quantify this dependence, a manipulation task involving the rotation of an icosphere is proposed and repeated with different levels of detail in the definition of the icosphere. Table II shows the total execution time of the local planner in an Intel Core 2 Duo T7500 (2.2GHz) computer with 2GB of RAM. By analyzing the data of Table 2, a quadratic relation is established between the number of faces and the execution time of the geometric planner. This relation indicates that the level of detail of the surface of the object can be adapted to the requirements of execution time. On one hand, when a real-time response is important, a simple representation of the surface of the object (usually composed by less than 1000 faces) should be used. On the other hand, when the planner can be executed off-line, the maximum level of detail for the object surface can be used. This result is one of the main advantages of the proposed algorithm because its computational cost can be adapted to the needs of the execution environment.

<table><caption>TABLE II. EVOLUTION OF EXECUTION TIME OF THE PLANNER</caption><thead><tr><td>Manipulated Object</td><td>Vertices</td><td>Faces</td><td>Time (s)</td></tr></thead><tbody><tr><td>Icosphere – 1 subdivision</td><td>12</td><td>20</td><td>0.506</td></tr><tr><td>Icosphere – 2 subdivisions</td><td>42</td><td>80</td><td>0.534</td></tr><tr><td>Icosphere – 3 subdivisions</td><td>162</td><td>320</td><td>0.588</td></tr><tr><td>Icosphere – 4 subdivisions</td><td>642</td><td>1280</td><td>0.649</td></tr><tr><td>Icosphere – 5 subdivisions</td><td>2562</td><td>5120</td><td>1.907</td></tr></tbody></table>

## V. CONCLUSION

This paper describes a novel planner for dexterous manipulation of objects by a multi-fingered hand. The planner computes the evolution of the fingers' joints by taking into account their kinematics and the contact maintenance. In fact, the planner implements a new contact evolution model based on a graph whose nodes represent contact states (i.e. pair of surface primitives that are in contact) and whose edges represent contact transitions generated by rolling movements between the primitives of these contact states.

In addition, another important contribution of this planner is the representation of the object and fingers surfaces as triangle meshes. This representation not only permits to apply the algorithm to many objects without imposing any limitation about their shapes as in previous manipulation planners, but it also permits to adapt the computational cost of the algorithm to the required execution environment by increasing or decreasing the number of primitives of the surfaces of the fingers and the object. The planner has been successfully tested in several real manipulation experiments.

In future work, the authors will include information about contact forces in the planner in order to guarantee that the contact is not lost when long manipulation movements are developed. In the current solution, a simple hybrid position-force controller is used to guarantee a minimum contact force. In addition, sliding contact should also be modeled.

## REFERENCES

[1] Hand and J. C. Trinkle, "Dextrous Manipulation by Rolling and Finger Gaiting," in *Proc. IEEE International Conference on Robotics and Automation*, Leuven, Belgium, 1998, pp. 730-735.

[2] A. M. Okamura, N. Smaby and M. R. Cutkosky, "An Overview of Dextrous Manipulation," in *Proc. IEEE International Conference on Robotics and Automation*, San Francisco, USA, 2000, pp. 255-262.

[3] B. Goodwine, "Stratified Motion Planning with Application to Robotic Finger Gaiting," in *IFAC World Congress*, China, 1999.

[4] B. Goodwine and J. Burdick, "Motion Planning for Kinematic Stratified Systems with Application to Quasi-static Legged Locomotion and Finger Gaiting," *IEEE Trans. Robot. Autom.*, vol. 18, no. 2, pp. 209-222, Apr. 2002.

[5] I. Harmati, B. Lantos and S. Payandeh, "On Fitted Stratified and Semi-Stratified Geometric Manipulation Planning with Fingertip Relocations," *Int. J. Robot. Res.*, vol. 21, no. 5-6, pp. 489-510, 2002.

[6] J. Xu and Y. Lou and Z. Li, "Hybrid Automation: A Better Model of Finger Gaits," in *Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems*, Beijing, China, 2006, pp. 4628-4633.

[7] J. Xu and Z. Li, "A Kinematic Model of Finger Gaits by Multifingered Hand as Hybrid Automaton," *IEEE Trans. Autom. Sci. Eng.*, vol. 5, no. 3, pp. 467-479, July 2008.

[8] S. M. LaValle and J. J. Kuffner, "Rapidly-exploring Random Trees: Progress and Prospects," in *Algorithmic and Computational Robotics: New Directions*, B. R. Donald, K. M. Lynch and D. Rus, Eds., Wellesley, MA, USA: A. K. Peters Ltd., 2001, pp. 293-308.

[9] M. Yashima, "Randomized Manipulation Planning Considering the Local Optimization Contact Mode Sequence," in *Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems*, Las Vegas, NV, USA, 2003, pp. 2914-2919.

[10] M. Yashima and H. Yamaguchi, "Complementary Formulation for Multi-fingered hand manipulation with Rolling and Sliding Contacts," in *Proc. IEEE International Conference on Robotics and Automation*, Taipei, Taiwan, 2003, pp. 2255-2261.

[11] J. P. Saut, A. Sahbani and V. Perdereau, "A Global Approach for Dextrous Manipulation Planning Using Paths in N-fingers Grasp Subspace," in *Proc. IEEE International Conference on Automation, Robotics and Vision*, Singapore, Singapore, 2006, pp. 2019-2024.

[12] J. P. Saut, A. Sahbani, S. El-Khoury and V. Perdereau, "Dextrous Manipulation Planning Using Probabilistic Roadmaps in Continuous Grasp Subspaces," in *Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems*, San Diego, CA, USA, 2007, pp. 2907-2912.

[13] J. C. Trinkle and J. J. Hunter, "A Framework for Planning Dextrous Manipulation," in *Proc. IEEE International Conference on Robotics and Automation*, Sacramento, CA, USA, 1991, pp. 1245-1251.

[14] H. Zhang, K. Tanie and H. Maekawa, "Dextrous Manipulation Planning by Grasp Transformation," in *Proc. IEEE International Conference on Robotics and Automation*, Minneapolis, MN, USA, 1996, pp. 3055-3060.

[15] M. Cherif and K. K. Gupta, "Planning Quasi-Static Fingertip Manipulations for Reconfiguring Objects," *IEEE Trans. Robot. Autom.*, vol. 15, no. 5, pp.837-848, Oct. 1999.

[16] M. Cherif and K. K. Gupta, "Global Planning for Dextrous Reorientation of Rigid Objects: Finger Tracking with Rolling and Sliding," *Int. J. Robot. Res.*, vol. 20, no. 1, pp. 57-84, Jan. 2001.

[17] D. J. Montana, "The Kinematics of Contact and Grasp," *Int. J. Robot. Res.*, vol. 7, no. 3, pp. 17-32, June 1988.

[18] A. Sahbani, S. El-Khoury and P. Bidaud, "An Overview of 3D Object Grasp Synthesis Algorithms," *Robotics and Autonomous Systems*, vol. 60, no. 3, pp. 326-336, March 2012.

[19] B. Gerkey, (2012, March). Gazebo Simulator. [Online]. Available: http://playerstage.sourceforge.net/gazebo/gazebo.html

[20] R. Smith, (2012, March). Open Dynamics Engine (ODE). [Online]. Available: http://opende.sourceforge.net/

[21] T. Moller, "A Fast Triangle-Triangle Intersection Test," *J. Graph. Tool*, vol. 2, no. 2, 1997.

[22] J. Xiao and X. Ji, "Automatic Generation of High-Level Contact State Space," *Int. J. Robot. Res.*, vol. 20, no. 7, pp. 584-606, July 2001.