Budapest University of Technology and Economics

Department of Control Engineering and Information Technology

Budapest,Hungary

OBJECT MANIPULATION PLANNING FOR

DEXTROUS ROBOT SYSTEMS

T

´

ARGYMANIPUL

´

ACI

´

O TERVEZ

´

ES ROBOTOK

´

ES

T

¨

OBBUJJAS KEZEK ESET

´

EN

Thesis by

G´abor Vass

In Partial Fulﬁllment of the Requirements for

the Degree of Doctor of Philosophy

Supervisors:

Dr.B´ela Lantos Dr.Shahram Payandeh

Dept.of Control Engineering and Information Technology School of Engineering Science

Budapest University of Technology and Economics Simon Fraser University

Budapest,Hungary Burnaby,British Columbia,Canada

May,2005

Declaration

Undersigned,G´abor Vass,hereby state that this Ph.D.Thesis is my own work wherein I

have used only the sources listed in the Bibliography.All parts taken from other works,

either in a word for word citation or rewritten keeping the original contents,have been un-

ambiguously marked by a reference to the source.

Nyilatkozat

Alul´ırott Vass G´abor kijelentem,hogy ezt a doktori ´ertekez´est magam k´esz´ıtettem ´es ab-

ban csak a megadott forr´asokat haszn´altam fel.Minden olyan r´eszt,amelyet sz´o szerint,

vagy azonos tartalomban,de ´atfogalmazva m´as forr´asb´ol ´atvettem,egy´ertelm˝uen,a forr´as

megad´as´aval megjel¨oltem.

Budapest,2005.05.16.

Vass G´abor

The reviews of this Ph.D.Thesis and the record of defense will be available later in the Dean

Oﬃce of the Faculty of Electrical Engineering and Informatics of the Budapest University

of Technology and Economics.

Az ´ertekez´esr˝ol k´esz¨ult b´ır´alatok ´es a jegyz˝ok¨onyv a k´es˝obbiekben a Budapesti M˝uszaki

´es Gazdas´agtudom´anyi Egyetem Villamosm´ern¨oki Kar´anak D´ek´ani Hivatal´aban el´erhet˝oek.

i

Acknowledgements

I wish to thank all the people who positively inﬂuenced my work on this thesis,provided me

with scientiﬁc advice and moral support.

I would like to thank Professor Dr.B´ela Lantos,my supervisor at the Department of

Control Engineering and Information Technology,Budapest University of Technology and

Economics,Budapest,Hungary.

Also,I would like to express my sincere gratitude to Professor Dr.ShahramPayandeh,my

supervisor at the School of Engineering Science,Simon Fraser University,Burnaby,British

Columbia,Canada,for his valuable advices and inspiring ideas.

Special thanks to Dr.B´alint Kiss and Dr.Istv´an Harmati for their valuable suggestions

and help.

I would like to thank to Bal´azs Hajnal,Gy¨orgy Balogh,Ervin T´oth,Zolt´an Proh´aszka,

Dr.Zsolt Kem´eny,Tam´as Falus,Mar´ıa del Carmen Carolina Am´ezquita Ben´ıtez and Javad

Dargahi for their support during my research works in Hungary and Canada.

I would like to thank to my family and Andrea Haraszti for their loving care and encour-

agement.

Finally,I would like to acknowledge the support for the research provided by the Natural

Sciences and Engineering Research Council of CANADA (NSERC) grant No.611205 and

the Hungarian National Research Programs under grant No.FKFP 0417/1997,INTCOM

TEMPUS JEP 12555-97 and OTKA T 029072,OTKA T 042634.

ii

K¨osz¨onetnyilv´an´ıt´as

Szeretn´ek k¨osz¨onetet mondani mindazoknak,akik a t´ezissel kapcsolatos munk´amat tudo-

m´anyos tan´accsal ´es t´amogat´assal pozit´ıvan befoly´asolt´ak.

Szeretn´ek k¨osz¨onetet mondani Dr.Lantos B´ela professzornak,a Budapesti M˝uszaki ´es

Gazdas´agtudom´anyi Egyetem Ir´any´ıt´astechnika ´es Informatika Tansz´ek´en dolgoz´o konzu-

lensemnek.

Szint´en szeretn´em kifejezni h´al´amat a kanadai Simon Fraser University,School of Engi-

neering Science tansz´ek´en dolgoz´o konzulensemnek,Dr.Shahram Payandeh professzornak

is ´ert´ekes tan´acsai´ert ´es inspir´al´o ¨otletei´ert.

Speci´alis k¨osz¨onet illeti Dr.Kiss B´alintot ´es Dr.Harmati Istv´ant hasznos javaslataik´ert

´es seg´ıts´eg¨uk´ert.

Szeretn´em megk¨osz¨onni a t´amogat´ast,amit Hajnal Bal´azs,Balogh Gy¨orgy,T´oth Ervin,

Proh´aszka Zolt´an,Dr.Kem´eny Zsolt,Falus Tam´as,Mar´ıa del Carmen Carolina Am´ezquita

Ben´ıtez ´es Javad Dargahi ny´ujtott Magyarorsz´agon ´es Kanad´aban.

Legf˝ok´epp h´al´as vagyok csal´adomnak ´es Haraszti Andre´anak a t´amogat´as´ert,amire a

hossz´u id˝o alatt nagy sz¨uks´egem volt.

V´eg¨ul szeretn´ek k¨osz¨onetet mondani a Natural Sciences and Engineering Research Coun-

cil of CANADA (NSERC) No.611205,a Fels˝ooktat´asi Kutat´asi ´es Fejleszt´esi Programok

FKFP 0417/1997,az INTCOMTEMPUS JEP 12555-97,az Orsz´agos Tudom´anyos Kutat´asi

Alapprogramok OTKA T 029072 ´es OTKA T 042634 programjait´ol kapott t´amogat´as´ert.

iii

Abstract

This Ph.D.Thesis deals with object manipulation and trajectory planning algorithms for

dextrous robot systems including cooperating robots and multiﬁngered hands.Three meth-

ods have been developed for object manipulation design and real-time trajectory planning.

The ﬁrst result is a new object manipulation algorithmusing artiﬁcial intelligence based

on simulated annealing and A* search.The upper level of the manipulation system,the

global planner generates the motion of the object.The lower level,the local planner deals

with the motion of the agents relative to the object and the design of the contact forces.

Simulated annealing can avoid the local minima in the energy function with high probability.

Application of the algorithm has been discussed for cooperating robots as mobile agents and

the combo of a robotic arm and a hand.

The second result assures further improvements in the manipulation design.A contact

point relocation method has been developed for manipulating agents in order to reconﬁgure

the grasp.The beneﬁt of the method is more stable manipulation and more ﬂexibility for

the agents.A temporary contact point on the object by a static obstacle of the environment

deals as pseudo-agent,the real agents can relocate the current contact points resulting in a

better position for the further manipulation.

The third result is a real-time method for motion planning with jerk,improving the

Minimum-time Spline-based Reduced State space (MSRS) approach.New algorithm has

been developed which optimizes the spline list of the path,reduces the time of the robot

motion and is fast enough to use it in real-time applications.It can be applied to optimize

in time robot trajectories of one or more robotic manipulators simultaneously.Modiﬁcation

of corner points at the end of the list or adding new ones can be performed real-time.The

optimized algorithm has been realized under RTOS-32 real-time operation system.

iv

Kivonat

Ez a Ph.D.´ertekez´es t´argymanipul´aci´os feladattal ´es trajekt´oria tervez´essel foglalkozik ko-

operat´ıv robotok ´es t¨obbujjas kezek sz´am´ara.T´argymanipul´aci´o ´es val´osidej˝u trajekt´oria

tervez´es t´emak¨or´eben h´arom m´odszer lett kifejlesztve.

Az ´ertekez´es els˝o eredm´enye egy ´uj t´argymanipul´aci´os algoritmus,amely mesters´eges

intelligencia m´odszereket haszn´al fel,´ugymint a szimul´alt leh˝ut´est ´es az A* gr´afkeres˝o

elj´ar´ast.A manipul´aci´os rendszer fels˝o szintje,a glob´alis tervez˝o a t´argy mozg´as´anak

param´etereit ´all´ıtja el˝o.Az als´o szint,a lok´alis tervez˝o feladata az ´agensek t´argyhoz vi-

szony´ıtott mozg´as´anak megad´asa ´es a kontaktus er˝ok megtervez´ese.Szimul´alt leh˝ut´essel

nagy val´osz´ın˝us´eggel elker¨ulhet˝ok az energia f¨uggv´eny lok´alis minimumai.Az ´ertekez´es be-

mutatja az algoritmus kooper´al´o robotokra mint mobil ´agensekre,valamint robotkar ´es k´ez

rendszerre val´o alkalmaz´as´at.

Az m´asodik eredm´eny a t´argymanipul´aci´o tervez´es tov´abbfejleszt´ese.A kontaktuspont

´athelyez˝o elj´ar´as a megfog´as ´atkonﬁgur´al´as´ara lett kifejlesztve manipul´al´o ´agensek sz´am´ara.

Seg´ıts´eg´evel stabilabb megfog´as ´es az ´agensek r´esz´ere nagyobb mozg´asi szabads´ag ´erhet˝o el.

A k¨ornyezetben tal´alhat´o,pszeud´o-´agensk´ent kezelt statikus akad´aly seg´ıts´eg´evel a t´argyon

´atmeneti kontaktuspont l´etrehoz´as´aval a val´odi ´agensek ´athelyezhetik az aktu´alis kontak-

tuspontokat,ami megk¨onny´ıtheti a tov´abbi manipul´aci´ot.

A harmadik eredm´eny egy val´osidej˝u trajekt´oria tervez˝o algoritmus,az MSRS m´odszer

tov´abbfejleszt´ese,amely ﬁgyelembe veszi a csukl´ok gyorsul´as deriv´altj´anak korl´atoz´asait is.

Az ´uj m´odszer optimaliz´alja a spline list´aval reprezent´alt robot trajekt´ori´akat,cs¨okkenti

a mozg´as idej´enek hossz´at ´es gyorsas´aga miatt val´osidej˝u rendszerekben is alkalmazhat´o.

Egy,vagy egyszerre t¨obb robot id˝o-optimaliz´alt trajekt´ori´ainak el˝o´all´ıt´as´ara is haszn´alhat´o.

Lehet˝ov´e teszi a sarokpontok megv´altoztat´as´at ´es hozz´aad´as´at a p´aly´ahoz menet k¨ozben,

val´os id˝oben.Az optimaliz´alt algoritmus RTOS-32 oper´aci´os rendszeren lett megval´os´ıtva.

v

Contents

Notation x

1 Introduction 1

2 Object manipulation 10

2.1 Introduction....................................10

2.2 Background of kinematic and force modeling..................14

2.2.1 Geometrical model............................14

2.2.2 Velocity and angular velocity model...................15

2.2.3 Static forces................................16

2.3 Relative motion of contacting rigid bodies...................17

2.4 Motion constraints................................19

2.5 Overview of the multi-agent planning system..................22

2.6 Random relative velocity generation.......................27

2.7 The V relative velocity matrix..........................28

2.8 Simulated annealing algorithm as a local planner...............29

2.9 Application for robotic manipulators......................33

2.9.1 Cooperating multiple robotic arms...................33

2.9.2 Puma robot with dextrous hand.....................34

2.10 Discussion.....................................37

2.11 Removing friction cone linearization.......................43

2.12 Conclusions....................................45

3 Agent relocation in the presence of a passive pseudo-agent 46

vi

3.1 Introduction....................................46

3.2 Contact point relocation.............................50

3.3 Static objects as pseudo-agents.........................54

3.4 Simulation results.................................59

3.5 Conclusions....................................62

4 Robot trajectory planning with jerk 64

4.1 Introduction....................................64

4.2 Review of the MSRS Jerk switching algorithm.................67

4.2.1 Spline equations..............................71

4.2.2 Motion limits along the path.......................74

4.2.3 Initial spline list..............................76

4.2.4 Spline local optimization.........................78

4.3 Optimized jerk switching.............................80

4.3.1 Steps of the optimization.........................84

4.4 Implementation results..............................88

4.5 Conclusions....................................89

5 Conclusions 91

Appendix 93

A.Review of Simulated annealing..........................93

B.Review of A* algorithm..............................95

C.Contact force design using LMI technique....................97

C.1 Background of LMI.............................97

C.2 Solvable problems using MATLAB LMI toolbox.............98

C.3 Friction constraints as LMIs........................98

C.4 LMI formulation of grasp constraints...................101

C.5 Interpretation of the results........................104

Bibliography 105

vii

List of Figures

1.1 TUB-PC Hand..................................5

2.1 Sliding and rolling motion............................11

2.2 The C

1

,C

2

contact frames............................18

2.3 Linearization of friction..............................21

2.4 Planning ﬂowchart of the system........................22

2.5 Task of the global planner............................23

2.6 Global planner pseudo-code...........................25

2.7 Reconﬁguration in 2D..............................27

2.8 The motion of the object and agents......................28

2.9 Generation of the velocity matrix V.......................30

2.10 Local planner pseudo-code............................32

2.11 Motion sequence of 3D agents..........................34

2.12 Three robotic manipulators grasping an object.................35

2.13 PUMA 560 arm with TUB-PC hand.......................36

2.14 Three diﬀerent motion trajectories of the agents and the object.......38

2.15 Contact forces and net travel for three motion sequences...........39

2.16 Motion sequence with workspace constraint...................40

2.17 Motion sequence with three manipulators.First left column,then right....41

2.18 Motion sequence with three manipulators.Top row ﬁrst,left to right.....44

3.1 Object reconﬁguration with three ﬁngers or three manipulators........47

3.2 Surface normal for curved objects........................48

3.3 Contact point relocation with 3D agents....................51

viii

3.4 Example for the objective function of the relocation algorithm........52

3.5 Object manipulation helped by a pseudo-agent.................54

3.6 System ﬂowchart for handling pseudo-agents..................55

3.7 Start contact with the pseudo-agent (obstacle).................57

3.8 Usage of pseudo-agent in 3D...........................58

3.9 Object manipulation applying a pseudo-agent.Top row ﬁrst,left to right..60

4.1 Example for the spatial path of the robot....................66

4.2 The spatial path as a function of λ.......................68

4.3 Spline sections...................................69

4.4 Two connected splines..............................70

4.5 Two base splines,with positive and negative acceleration...........71

4.6 Path partitioning.................................76

4.7 Initial spline list construction..........................78

4.8 Local optimization for a spline..........................79

4.9 Example for μ (velocity) and ˙μ (acceleration) optimization..........81

4.10 Possible acceleration proﬁles for increasing velocity..............82

4.11 Steps of the optimization.............................85

4.12 Forward optimization pseudo code.......................87

4.13 Velocity and acceleration of a joint before optimization (original MSRS)...88

4.14 Velocity and acceleration of a joint after optimization (optimized MSRS)..88

4.15 Trajectory planning without optimization (original MSRS)..........89

4.16 Trajectory planning with optimization (optimized MSRS)...........89

A.1 Temperature schedule for accelerated SA....................94

ix

Notation

A

#

pseudo-inverse of A

α maximum positive acceleration in the spline

β maximum negative acceleration in the spline

B base frame

B

A

base frame of the arm

C contact frame

C

i

the i

th

contact frame on the object

E energy function for simulated annealing

E

a

bc

an A×A symmetric matrix with elements BC=1,CB=1 and all the others 0.

f force in a contact point

f

O,ext

resultant of the external forces applied on the object

f

i

x

x direction tangential force in the i

th

contact point

f

i

y

y direction tangential force in the i

th

contact point

f

i

z

z direction normal force in the i

th

contact point

F

i

the i

th

ﬁnger frame

F

p

the frame of the pseudo-agent

γ jerk (ﬁrst derivative of acceleration) along the path

Γ a directed graph used for search by the A* algorithm

G Grasp matrix

k the number of agents

K

c

weighting constant for collisions

K

e

weighting constant for force equilibrium

K

f

weighting constant for absolute force values

x

K

g

weighting constant for velocity changes

K

r

weighting constant for relocations

L

i

contact frame on the i

th

ﬁnger

μ coeﬃcient of friction;velocity along the path

n number of intervals between two subgoals

O object frame

ω

i

angular velocity of agent i relative to the object

P palm frame

R

Ψ

orientation matrix between two contact frames

S

i

the i

th

subgoal

S

n×n

set of n×n symmetric matrices

t

s

time interval between two subgoals

T simulated annealing temperature parameter

T

B,F

i

transformation between the base coordinate frame and the ﬁngertip frame

T

B,O

transformation between the base frame and the object frame

T

F

i

,L

i

transformation between the i

th

ﬁngertip frame and the ﬁnger contact frame

T

O,C

i

transformation between the object frame

and the i

th

contact frame on the object

T

Ψ

i

orientation matrix between the contact frame of the object

and of the i

th

ﬁngertip

u 2D Gauss frame coordinates of the 3D surfaces

V relative velocity matrix,size n ∗ k

v

i

linear velocity of the i

th

agent relative to the object

v

ij

relative velocity of the i

th

agent in the j

th

interval

Kernel(A) kernel (or null) of A

Range(A) range (or image) of A

xi

Chapter 1

Introduction

Moving an object with a robotic hand is a diﬃcult problem.First collision free paths

for all ﬁngers of the hand have to be found,then forces in the contact points must be

generated.Finally the generated path is executed considering the robot geometry and joint

parameters.The ﬁngertips of a dextrous robotic hand can be treated as manipulating agents

with motion constraints,thus object manipulation algorithms with multiple agents can be

applied to dextrous robotic hands.The manipulation task (called object re-conﬁguration

problem) is stated as the following:given an initial grasp of the object ﬁnd the motion’s

trajectories of the ﬁngertips to move the object to the desired conﬁguration.The forces

exerted at the contact points by the ﬁngertips are determined ﬁrst to ensure a stable grasp,

then to manipulate the object.

Recently numerous object manipulation algorithms have been developed (see Cherif and

Gupta (1997)).The topic of object manipulation with agents or robotic hands can be

classiﬁed by the mode of the relative motion of the agents and the object.Pushing,rolling

and sliding are examples of relative motions which the agents can produce with respect to

the object.The relative motion between the ﬁngertips and the object determines the whole

motion of the system,if the desired motion of the object is known.

Several aspects of the basic mechanics of manipulating objects are explored by Salisbury

and Mason (1985).The book deals with contact modeling,hand mechanisms,force applica-

tion and velocity analysis,stiﬀness control and sensing,manipulator grasping and pushing

operations.

1

2 CHAPTER 1.INTRODUCTION

Han and Trinkle (1998) present a general framework of dextrous manipulation planning.

In the framework three hemispherical ﬁngertips are used to demonstrate the concept of ﬁnger

gaiting and rolling.The group of ﬁngers is partitioned during the manipulation into grasping

ﬁngers and free ﬁngers.Two ﬁnger gaiting primitives are introduced:ﬁnger rewind limiting

ﬁngers back to their workspace and ﬁnger substitution to remove the limiting ﬁnger from

the grasp.

A wide survey of the ﬁeld of artiﬁcial intelligence can be found in Russel and Norvig

(1995).The book describes the basic concepts of intelligent agents,problem solving,know-

ledge and reasoning,sensing,and natural language communication.Discusses several search

algorithms,the construction of knowledge bases and surveys the ﬁeld of learning.Soft-

computing methods such as neural networks,fuzzy logic,simulated annealing,genetic algo-

rithms,reinforcement learning are discussed.

Latombe (1991) discusses the problem of autonomous robots.The central theme of the

book is motion planning.The robot should decide what motions to perform in order to

achieve the goal conﬁguration.The robot accomplishes tasks by moving in its workspace.

The most necessary property of the autonomous robot is the ability to plan its own motions.

Other topics covered in the book are roadmap methods,algorithms for cell decomposition,

potential ﬁeld methods,multiple moving objects,dealing with uncertainty and movable

objects.

Recently,in the ﬁeld of object manipulation a relatively new concept of stratiﬁed control

has appeared.A representative class of stratiﬁed control applications are the manipulation

systems.Harmati,Lantos,and Payandeh (2000a) provide a basic concept for object manip-

ulation with ﬁnger relocation using stratiﬁed control approach for nonlinear systems where

conﬁguration space is separated into smooth surfaces.

The object manipulation task is divided into three categories:

• the goal of the object manipulation task is to reach the desired object conﬁguration

without considering the contact conﬁguration;

• the grasp adjustment attains the desired contact conﬁguration by disregarding the

object conﬁguration;

3

• dextrous manipulation leads the robotic hand to its ﬁnal state taking into account the

desired object and contacts conﬁguration,respectively.

Harmati,Lantos,and Payandeh (2005) concerns with object and dextrous manipulation,

proposes two methods to lead the object into a desired conﬁguration while the ﬁngertips

reach the desired contact points on the surface.Both of them rely on stratiﬁed motion

planning.The methods are restricted in a way to reach arbitrary contact points.The

proposed methods allow no rolling and sliding relative motion between the object and the

ﬁngertips but ﬁnger relocation is allowed.

Force closure stability of the grasping is guaranteed by suﬃcient number of ﬁngers.In the

paper an example of dextrous manipulation is shown for a four ﬁngered robotic hand and an

object of smooth surface.An overview is given about the stratiﬁed motion planning,which

is based on a smooth motion planning and operates on sequences of ﬂows.The solution of

the smooth and stratiﬁed motion planning problem consists of the sequence of ﬂows along

the vector ﬁelds of the system.The described manipulation algorithms are based on a simple

ﬁctitious (ﬁtted) system that reduces the complexity of the computations.Hence,they make

the interpretation of the state trajectories easy from the results in the conﬁguration space.

The ﬁrst proposed method is based on the philosophy of the stratiﬁed motion planning and

uses a special ﬁctitious systemcalled ﬁtted system.The special parameterization of the ﬁtted

system yields simple vector ﬁelds where one also can easily check any system’s property (e.g.

stratiﬁed controllability).The method is able to carry out a dexterous manipulation in the

restricted workspace of the ﬁngertips.However,the method does not assure automatically

the force closure stability and there is no protection against ﬁnger collisions.

The second method is a semi-stratiﬁed motion planning on a ﬁtted system using task

decomposition Harmati,Lantos,and Payandeh (2000b).Beside the stratiﬁed motion plan-

ning,the semi-stratiﬁed motion planning includes a strategy for systematic ﬁnger relocations.

The strategy is based on a subsegment generation procedure providing reference ﬁngertip

positions to a desired object motion,which can be chosen suitably.Thus,the method has

restrictions,arbitrary ﬁngertip positions on the object can not be achieved.It causes that

dexterity in manipulation fails,hence it aims primarily the object manipulation.However,

the method provides force closure stability,collision avoidance and a greater degrees of free-

4 CHAPTER 1.INTRODUCTION

dom in ﬁnger relocation than the ﬁrst method because it allows any trajectory in the free

space for the ﬁngertips.The simulation results are demonstrated through a manipulation

example of an egg by four ﬁngers.

Adiﬀerent approach of trajectory planning for dextrous manipulation with rolling contacts

is shown in Kiss,L´evine,and Lantos (1999).In this paper the case of one-point rolling

contacts is considered without slipping between the surfaces which give rise to kinematic

constraints in the model.Since these constraints are not integrable in general hand-object

systems belong to the larger class of nonholonomic mechanical systems.

The ﬂatness property of the system simpliﬁes the parametrization of feasible trajectories.

A system is diﬀerentially ﬂat,if one can ﬁnd a set of diﬀerentially independent variables

(called the ﬂat output) such that every variable in the system (including the inputs) are

functions of the ﬂat output and a ﬁnite number of its time derivatives.The ﬂat output is a

function of state variables,the input variables and the input derivatives up to a ﬁnite order.

The ﬂat output has the same number of components as the input vector.As a consequence of

the deﬁnition of the ﬂat output,trajectory planning is reduced to an interpolation without

need to integrate the system.This property makes the planning applicable in real-time

systems.

In the article it is proven that the ﬂatness property holds to the entire hand-object system

if one can integrate the nonholonomic constraints.An example of a planar hand-object

system with a special geometry is presented in details.Moreover,a reduced equivalent

system can be obtained by introducing a virtual ﬁnger.The key of the planning problem is

to ﬁnd integrable nonholomic equations.

A good book exploring parallels in sensimotor integration in dextrous robot and human

hands is Venkataraman and Iberall (1990).The book presents works of researchers in dex-

trous manipulation.Murray,Li,and Sastry (1994) gives a mathematical introduction to

robotic manipulation.The selection of topics covers kinematics and dynamics of robots,

grasping and manipulation of objects by multiﬁngered robot hands and nonholonomical

motion planning.

The Department of Control Engineering and Information Technology (CEIT,former

Process Control Department,PC) at the Budapest University of Technology (BUTE) launch-

5

ed a project on the construction and manufacturing of a new type of dextrous gripper in

1995 (see 1.1).Details of mechanical construction,hand kinematics and dynamics were elab-

orated by Ludvig (1997).The mechanical interface of the TUB-PC hand is rather universal,

therefore the hand can be ﬁtted any PUMA or SCARA type robots.

Figure 1.1:TUB-PC Hand

The TUB-PC hand is equipped with three ﬁngers,arranged symmetrically on a pla-

nar palm.Fingers are identical,each ﬁnger has four degrees-of-freedom according to the

TRR⊥R joint formula.The rotational joints are tendon operated,driven by miniature

DC motor-planetary gear units,12 actuators in total.Drives are ﬁtted to a proximal segment

of the robot.Translational slides can be positioned only manually with turn knobs mounted

to spindles.The mechanics of the hand was ﬁnished by 1997.

The control system is divided into subsystems.The ﬁrst subsystem,developed by the

Department of Automation and Applied Informatics (AAI) of BUTE,is the robot control

system consisting of a host PC with extension rack and the power electronics of the axes

servo systems (analog PI current controllers with PWM).The extension rack contains three

special ARC boards.Every board contains an i386EX processor,a DSP (TMS320C31)

for the realization of the robot control algorithms,a tacho-processor receiving the encoder

signals of four axes and DACs for the reference signals of the low level current (torque)

control loops of four axes.The boards are connected to the host PC by dual port RAMs.

6 CHAPTER 1.INTRODUCTION

The communication amongst the boards can be performed by using dual port RAMs or

CAN-bus.The calibration process of the robot is supported by a PCL analog input card

of the host PC.The ARPS robot programming language,developed by CEIT,runs on the

host PC under QNX real-time operation system.QNX is a Posix-like,multiuser,multitask

operation system,frequently used in industrial applications.

The robot control subsystem is designed to be capable to perform diﬀerent type of robot

control algorithms:decentralized cascade position and velocity control,self-tuning adaptive

control,computed torque technique,hybrid position and force control,etc.The control

system of the robot is described in Tevesz (1998).

The hand control subsystemof the tendon-driven TUBdextrous hand,developed by CEIT,

consists of a PC with extension rack containing an analog input card (PCL-818 with 16

inputs) for the analog position signals and an analog output card (PCL-727 with 12 outputs)

for the servo ampliﬁers of the driving motors of the tendons.The servo ampliﬁers of the

motors are removed to a separate rack.Each ﬁnger is driven by 4 tendons.The tendon

controller PC is running under QNX and communicates with the host PC by Ethernet.At

the actual level the hand control programperforms the path design in Cartesian coordinates,

the solution of the inverse kinematics,the conversion of the joint variables of the ﬁngers into

the position angles of the motors and the coordinated position control of the motors driving

the tendons (4 motors are driving 3 ﬁnger joints in a coordinated way).Algorithms for the

kinematics and dynamics of the hand were discussed in Ludvig (1997).

The low-level controller program runs on a PC.Inverse kinematic solutions for all the

ﬁngers and the arm are needed,the joint coordinates are computed real time from the

Cartesian coordinates of the ﬁngertips.Only the joint coordinates are observed and tendon

forces are measured,other sensors are not used yet.In the future contact force sensors will

be attached to the ﬁngertips.

The controlling system consists of seven processes,which run using time share,the

scheduling of the processor time is handled by the main process.Five processes handle

the kinematics of the ﬁngers (one for each ﬁnger),the hand control law,and the informa-

tion exchange between robot and hand controllers.The remaining process reads the oﬀ-line,

previously planned path.Because of multiple processes,the communication among them is

7

an important part of the system.

The low level part of the motion planning system uses the graphic interface of QNX;the

signals of the hardware are observable.The low level consists of the following components:

• direct and inverse geometry transformations between the joint coordinates and the

Descartes coordinates of the ﬁngertips

• a decentralized position-force controller (for example with PID controller)

• user interface for observing the hardware signals

• hardware handlers (A/D,D/A converters)

The early control programs of the hand including a calibration tool,a path-planning program

and decentralized PID controllers of the hand was developed by the author as an M.Sc thesis

(Vass (1997)).

Currently the hand can be tested separately and further development of the motion plan-

ning system of the hand can be made.An early version of the motion design of the multi-

ﬁngered robotic hand can be found in Vass (1998).

The hand was developed as a part of an integrated system whose completion is in progress

in the Hungarian National Research program OTKA T 042634.The system consists of a

PUMA 560 armhaving six degrees of freedom,the TUB-PC Hand and a stereo vision system

(see Lantos,Klatsm´anyi,Ludvig,and T´el (1997),T´el and Lantos (2003)).

Having the trajectories of the motion of a robotic system planned,the motion should be

executed.Vass,Payandeh,and Lantos (2003) presents a real-time optimized robot trajectory

planning,which takes into account the maximum derivative of the acceleration of the joints.

Outline and Contribution of this Dissertation

The objective of the thesis is to develop a framework of a manipulation planner in the

presence of obstacle constraints between the initial and goal conﬁguration of the object.In

this approach,ﬁngertips can loose and regain contact with the object at some other planned

next location.The controller devises a strategy where the environmental constraints (e.g.

obstacles) can also be used as passive contact surfaces supporting the manipulation of the

8 CHAPTER 1.INTRODUCTION

object.The algorithm can also be applied to the design of object reconﬁguration with

multiple agents including cooperating robots.Only pure rolling and pure sliding relative

motions between the ﬁngertips and the object and ﬁnger relocation are assumed.The planner

allows breaking contact between a ﬁngertip and the object due to ﬁnger relocation.

In the thesis the following problems are examined:

• A quasi-static relative motion generation method between the object and the robot,

using simulated annealing algorithm.

• Possible ﬁnger relocation on the object during the motion.

• Usage of surfaces of the environment to support the motion of the object.

• An improved near time-optimal trajectory design algorithm,which considers the max-

imum velocity,acceleration and jerk values of each joint of the robot.

In chapter 2 a model based motion planner algorithm for manipulating agents for object

re-conﬁguration is presented.The motion sequence is represented by a relative velocity

matrix.The motion of the agents relative to the object can be pure sliding and pure rolling.

The described method uses simulated annealing (SA) for generating the relative motion

between the object and agents.The algorithm can be used for example to move a known

shaped object to a diﬀerent position and orientation with a robotic hand.

Chapter 3 describes two extensions for the object reconﬁguration method.The ﬁrst

contribution is a method for contact point relocation,which allows the manipulating agent

to break contact and later ﬁnd a new contact point on the object being manipulated.

The second extension is the usage of static obstacles in the environment of the manip-

ulating system as pseudo-agents.The two extension can be used together to improve the

robustness and ﬂexibility of the planner.

Path planning for a robot is the generation of subsequent robot positions which must

be followed during the motion.Beside that,the time properties of the motion should be

produced.This is the task of the trajectory planner.

Executing the planned spatial trajectories of the motion of the robot and object,the

dynamic properties of the robot must be considered.The maximum velocity,acceleration

9

and jerk of each joint of the robot determine additional limits of the motion.The real-

time trajectory generation should produce near time-optimal motion considering all of these

limits.

After computing a sequence of desired object and robot positions,the distribution of the

time parameter along the path is required for the realization of the motion.During the

motion these positions are given to the robot controller,which should be able to follow the

nominal path if it satisﬁes the motion constraints.

Chapter 4 contains a trajectory planning algorithmwhich takes into account the maximum

jerk (the ﬁrst derivative of acceleration).The contribution of the chapter is the extension of

the Minimum-time Spline-based Reduced State space (MSRS) approach of robot trajectory

planning.The presented algorithm modiﬁes the motion trajectories generated by the MSRS

approach in order to decrease the time required for the robot motion.Modiﬁcation of the

spatial trajectory on the ﬂy or adding new parts can be performed real-time.The algorithm

is fast enough to use in real-time applications.

Finally,chapter 5 summarizes the results.Fundamental deﬁnitions and concepts of sim-

ulated annealing,A* search algorithm and friction cone linearization removal based on LMI

used in the thesis are outlined in Appendix.

Chapter 2

Object manipulation

2.1 Introduction

Object manipulation either by multiple agents or dextrous robotic hand is a center ﬁeld in

robotic research.In this chapter a motion planner for object reconﬁguration with multiple

dextrous agents is proposed.

Object manipulation ﬁrst requires that the object be located within its environment.

Then,the object must be initially grasped by the agents.When the grasp is stable,the

manipulation process can be started.The manipulation task (called object reconﬁguration

problem) is stated as the following:given an initial grasp of the object ﬁnd the motion’s

trajectories of the agents to move the object to the desired conﬁguration.In general collision

free paths for all agents must be found toward the contact points on the object (pre-grasp

conﬁguration) and the grasping and manipulation forces should then be exerted on the object

by the agents.These forces are determined ﬁrst to ensure a stable grasp,then to manipulate

the object.

A survey of robot grasp synthesis algorithms is described in Shimoga (1996).The object

manipulation problem can be divided into two sub-problems:the global planner and the

local planner (see Cherif and Gupta (1997)).The task of the global planner is to search for

nominal path in the conﬁguration space among static obstacles between the initial and the

desired position and orientation of the object by generating points (subgoals) in the object’s

conﬁguration space.The local planner tries to ﬁnd admissible motion of the agents between

10

2.1.INTRODUCTION 11

the subgoals.The detailed trajectories of the agents’ motion (path) between two subgoals

are generated by the local planner.

Numerous object manipulation algorithms have been proposed,one of the most frequent

model used in motion planner algorithms is the description of the motion of two contacting

object which is given in Montana (1988).An algorithmfor four-ﬁnger equilibrium and force-

closure grasps of polyhedral objects is proposed by Ponce,Sullivan,Sudsang,Boissonnat,

and Merlet (1997),another algorithm for dextrous grasping force optimization is given by

Buss,Faybusovich,and Moore (1998).

In Chen,Walker,and Cheatham (1995) an analysis of the mechanics for grasps of solid

objects is presented.The contact force decomposition at each contact points results in a new

formulation relating the contact forces and the geometry of contact points.The functions of

the normal and tangential contact force components are visualized as squeezing and twisting

through this new formulation.

The topic of object manipulation with agents or robotic hands can be classiﬁed by the

mode of the relative motion of the agents and the object.Pushing,rolling and sliding are

examples of relative motions which the agents can produce with respect to the object.In

this chapter only pure rolling and pure sliding relative motions between the agents and the

object are assumed (ﬁg.2.1).The two relative motion types have diﬀerence in the exerted

forces in the contact points (denoted by arrows in the ﬁgure):sliding requires contact forces

outside of the cone of friction,otherwise the motion will be rolling.

Figure 2.1:Sliding and rolling motion

Instead of randomselection proposed in Cherif and Gupta (1997) and Vass,Payandeh,and

12 CHAPTER 2.OBJECT MANIPULATION

Lantos (1999c),simulated annealing algorithmcan be used at the local planner level to choose

relative velocities for the agents (see Vass,Payandeh,and Lantos (1999a).The purpose of

using simulated annealing is to avoid local minima of the motion planning objective function

which exist when there are various constraints required for deﬁning feasible motions and

forces.Simulated annealing is a general-purpose optimization tool which can be easily used

with any constraints in the searching space.A short introduction to simulated annealing

is presented in the Appendix.Motion planning can be treated as an optimization problem,

therefore simulated annealing is useful if the energy function of the searching space has many

minima.

There are several examples for application of soft-computing in robotics.Simulated an-

nealing is frequently applied on the strategic level of motion planning,for example to comply

with task constraints.A near time-optimal inspection-task-sequence planning for two co-

operative industrial robots is outlined in Cao,Dodds,and Irwin (1998).The object to be

observed is moved by one of the robots,while the other handles the inspection tool.The

objective of the task-sequence planning is to ﬁnd a series of near time-optimal ﬁnal con-

ﬁgurations for two arms where the inspection operations are undertaken for segment (link)

motions and ﬁnd a near time-optimal task sequence of inspection points.The task-sequence

planning is formulated as a variation of the traveling salesman problem (see Ansari and Hou

(1997)),and simulated annealing is used to ﬁnd a near time-optimal route.

In Lee and Kardaras (1997) an eﬃcient path planning approach is presented in which a

path is represented by a series of via points connected by elastic strings that are subject

to displacement due to collisions with obstacles as well as constraints pertaining to the

domain to which path planning is applied.Obstacle regions are represented by a potential

ﬁeld.Simulated annealing approach is used for local minima problems from the potential

ﬁeld.The potential ﬁeld is generated by a multi-layered neural network which implements

simulated annealing with the temperature T of its activation functions.

In Qin and Cameron (1996) a motion planning system for manipulators in a 3D envi-

ronment using an embedded expert system is presented.The system represents heuristic

knowledge as a number of rules in a discretized conﬁguration space (C-space).The expert

system performs geometric reasoning on the C-space so that only a small fraction of conﬁg-

2.1.INTRODUCTION 13

uration is needed to be checked.The algorithm is fast,for manipulators with low degree of

freedom it provides on-line motion planning.In more complicated cases the system exploits

simulated annealing.

Rus (1999) introduces an algorithm for object reorientation called ﬁnger tracking.It is

used for in-hand manipulation of rigid,piecewise smooth 3-D objects.The object manipu-

lation problem when only the ﬁngers and the object move,but not the arm is called in-hand

manipulation.The basic idea of the method is that the ﬁngers cooperate to reorient the

object by applying a normal force while sliding compliantly on the object surface.The dif-

ferential control for ﬁnger tracking is described and analyzed,and extended to a continuous

control for a set of cooperating robot ﬁngers.

A separate problem within the object manipulation problem is determining the contact

forces which assure force closure against disturbances in any possible directions if the contact

points have already been found.This problem is a nonlinear programming (NP) problem

if the friction cone is not approximated by linear constraints.Since this NP subproblem is

a numerically expensive low level subproblem within the object manipulation using ﬁnger

relocation hence we use ﬁrst linear approximation of the friction cones allowing the appli-

cation of linear programming (LP) method for the low level optimization subproblem.The

friction cone approximation can be removed by using linear matrix inequality (LMI) method

ﬁrst used by Han,Trinkle,and Li (2000) for grasp analysis.LMI technique allows the use

of interior point polynomial algorithms in convex programming (Nesterov and Nemirovsky

(1994)).

The topic of dynamic control of 3-D rolling contacts in two-arm manipulation is studied

by Sarkar,Yun,and Kumar (1997).In order to obtain a dynamic model of the manipulation

system,the velocity and acceleration equations for three-dimensional rolling contacts are

derived.A nonlinear feedback control algorithm that decouples and linearizes the system is

shown.The dynamic control of 3-D sliding contacts with multiﬁngered hands is presented

by Zheng,Nakashima,and Yoshikawa (2000).The static and dynamic frictions between

the sliding ﬁnger and the surface of the object are considered and their eﬀects regarding to

the behavior of the manipulating system are discussed.A dynamic control law for realizing

the desired object motion is proposed.The realizability of the given trajectories under the

14 CHAPTER 2.OBJECT MANIPULATION

constraints of maximal static and dynamic frictions and of joint driving torque are discussed.

The objective of this chapter is to present a quasi-static simulated annealing based motion

planner which uses a proposed relative velocity matrix to deﬁne the motion sequence of

the object and the agents.The organization of the chapter is as follows:in section 2.2

an outline of the kinematic modeling is given,then in section 2.3 the relative motion of

contacting objects is described.In section 2.4 the motion constraints are introduced.In

section 2.5 the overview of the planning system is presented.The application of a random

generation method is outlined in section 2.6.In section 2.7 the relative velocity matrix is

introduced.Section 2.8 describes the use of simulated annealing with the relative velocity

matrix.Application of the algorithm for robotic manipulators is shown in 2.9.Section

2.10 discusses the results.Section 2.11 shows a solution for removing the friction cone

linearization,and ﬁnally the conclusions of the chapter are presented in section 2.12.

The results of this chapter are published in Vass,Payandeh,and Lantos (1999a),Vass,

Payandeh,and Lantos (1999c),Vass,Payandeh,and Lantos (1999d),Vass,Payandeh,and

Lantos (2000c),Vass and Toth (2001) and Kiss and Vass (2001).

2.2 Background of kinematic and force modeling

2.2.1 Geometrical model

Let K

1

and K

2

denote two orthonormal coordinate frames,T

12

is the homogeneous trans-

formation between K

1

and K

2

,r

1

and r

2

vectors pointing to the same point subscribed in

the base of K

1

and K

2

respectively,then

⎛

⎝

r

1

1

⎞

⎠

=

⎡

⎣

R

12

p

12

0

T

1

⎤

⎦

⎛

⎝

r

2

1

⎞

⎠

= T

12

⎛

⎝

r

2

1

⎞

⎠

(2.1)

Consider the special case of C

i

contact point between the object and the i

th

ﬁngertip

of a robotic hand.Let B,O,F

i

,C

i

,L

i

denote the base frame,the object frame,the i

th

ﬁnger frame,the i

th

contact frame on the object,and the contact frame on the i

th

ﬁnger

respectively.

Assume that the direction of the surface normal of the common tangential plane between

2.2.BACKGROUND OF KINEMATIC AND FORCE MODELING 15

the ﬁngertip and the object at the contact point is equal to the direction of the z-axes of

L

i

and C

i

(the external normal of the object is +z

C

i

and −z

L

i

).Assume,that x

L

i

can be

rotated around z

C

i

into the direction of x

C

i

by angle Ψ

i

:

R

Ψ

i

=

⎡

⎢

⎢

⎢

⎣

cos Ψ

i

−sinΨ

i

0

−sinΨ

i

−cos Ψ

i

0

0 0 −1

⎤

⎥

⎥

⎥

⎦

=

⎡

⎣

ˆ

R

Ψ

i

0

0 −1

⎤

⎦

(2.2)

Let p

O,C

i

and p

F

i

,L

i

denote the position part of the homogeneous transformations between

frames O and C

i

and between frames F

i

and L

i

respectively.The transformation between

the contact frames can be written as:

T

B,O

⎛

⎝

p

O,C

i

1

⎞

⎠

= T

B,F

i

⎛

⎝

p

F

i

,L

i

1

⎞

⎠

(2.3)

where T

B,O

and T

B,F

i

denotes the homogeneous transformations between frames B and O

and between frames B and F

i

,respectively.

2.2.2 Velocity and angular velocity model

Consider two rigid bodies with attached K

1

and K

2

frames.The connection between the

linear and angular velocities of the two origins of the frames in basis-dependent form can be

written as:

2

v

2

= R

T

12

(

1

v

1

+

1

ω

1

×p

12

) = R

T

12

1

v

1

−R

T

12

[p

12

×]

1

ω

1

2

ω

2

= R

T

12

1

ω

1

(2.4)

where [a×] is the matrix of the vector product.The bottom-right index deﬁnes the point,

of which linear and angular velocities are considered.The top-left index identiﬁes the basis-

vectors.If it is apparent from the environment,then the top-left indices are omitted.The

linear and angular velocities are measured relative to the ﬁx B base frame.The relative

linear and angular velocities v

i

and ω

i

between the i

th

ﬁnger and the object in the common

contact point satisfy the following equations:

⎛

⎝

C

i

v

C

i

C

i

ω

C

i

⎞

⎠

=

⎛

⎝

C

i

v

L

i

C

i

ω

L

i

⎞

⎠

+

⎛

⎝

C

i

v

i

C

i

ω

i

⎞

⎠

=

⎡

⎣

R

Ψ

i

0

0 R

Ψ

i

⎤

⎦

⎛

⎝

L

i

v

L

i

L

i

ω

L

i

⎞

⎠

+

⎛

⎝

C

i

v

i

C

i

ω

i

⎞

⎠

(2.5)

16 CHAPTER 2.OBJECT MANIPULATION

The kinematics of the object and the i

th

ﬁnger can be written as:

⎛

⎝

C

i

v

i

C

i

ω

i

⎞

⎠

=

⎡

⎣

R

T

O,C

i

−R

T

O,C

i

[p

O,C

i

×]

0 R

T

O,C

i

⎤

⎦

⎛

⎝

O

v

O

O

ω

O

⎞

⎠

(2.6)

⎛

⎝

C

i

v

L

i

C

i

ω

L

i

⎞

⎠

=

⎡

⎣

R

Ψ

i

0

0 R

Ψ

i

⎤

⎦

⎡

⎣

R

T

F

i

,L

i

−R

T

F

i

,L

i

[p

F

i

,L

i

×]

0 R

T

F

i

,L

i

⎤

⎦

⎛

⎝

F

i

v

F

i

F

i

ω

F

i

⎞

⎠

(2.7)

Let V

F

i

and V

O

denote the spatial velocity of the i

th

ﬁnger and the object respectively.The

Jacobi matrices and the spatial velocities are written as:

J

C

i

=

⎡

⎣

R

T

O,C

i

−R

T

O,C

i

[p

O,C

i

×]

0 R

T

O,C

i

⎤

⎦

J

L

i

=

⎡

⎣

R

Ψ

i

0

0 R

Ψ

i

⎤

⎦

⎡

⎣

R

T

F

i

,L

i

−R

T

F

i

,L

i

[p

F

i

,L

i

×]

0 R

T

F

i

,L

i

⎤

⎦

J

O

= [J

T

C

1

,...,J

T

C

k

]

T

J

H

= diag(J

L

1

,...,J

L

k

)

V

H

= coll(V

F

1

,...,V

F

k

)

V

r

= J

O

V

O

−J

H

V

H

(2.8)

2.2.3 Static forces

Consider a static force f

i

and torque m

i

in the contact point in the C

i

frame.The equivalent

forces and torques can be described in the O frame origin in basis-independent form:

f

O

= f

C

i

m

O

= m

C

i

+p

O,C

i

×f

C

i

(2.9)

In basis-dependent form:

⎛

⎝

O

f

O

O

m

O

⎞

⎠

=

⎡

⎣

R

O,C

i

0

[p

O,C

i

×]R

O,C

i

R

O,C

i

⎤

⎦

⎛

⎝

C

i

f

C

i

C

i

m

C

i

⎞

⎠

= J

T

C

i

⎛

⎝

C

i

f

C

i

C

i

m

C

i

⎞

⎠

(2.10)

Let deﬁne the spatial net contact forces:

F

C

= coll(F

C

1

,...,F

C

k

) where F

C

i

=

C

i

f

T

C

i

,

C

i

m

T

C

i

then F

O,ext

+J

T

O

F

C

= 0 (2.11)

Assuming quasi-static manipulation,F

O,ext

is the gravity,which aﬀects the object.

2.3.RELATIVE MOTION OF CONTACTING RIGID BODIES 17

2.3 Relative motion of contacting rigid bodies

The relative motion of two contacting rigid and piecewise smooth bodies is solved by Montana

(1988),by describing the motion of the contact point on the bodies’ surfaces.In the following

the outline of the Montana equations is introduced.

First,deﬁne a surface S ⊂

3

.A coordinate path S

0

⊂ S is an open and connected

surface,where there exists an open subset U ⊂

2

and an invertible map f:U →S

0

such

that the partial derivatives f

u

(u) and f

v

(u) are linearly independent for all u = (u,v) ∈ U.

In other words f is a

2

→

3

mapping function.

A coordinate system for S

0

is the pair of (f,u) and (u,v) = f

−1

(s) denote the coordinates

of a point s ∈ S

0

.A two-manifold embedded in

3

(called a manifold ) is a surface S ⊂

3

that can be written S =

n

i=1

S

i

,where S

i

is a coordinate patch for S.The set {S

i

}

n

i=1

is

called an atlas for S.

A Gauss map for a manifold S is a continuous map g:S →S

2

⊂

3

such that for every

s ∈ S,g(s) is perpendicular to S at s.Consider a manifold S with Gauss map g,coordinate

patch S

0

for S,and coordinate system (f,U) for S

0

.If f

u

(u) · f

v

(u) = 0 for all u ∈ U,then

the coordinate system (f,U) is orthogonal.For an orthogonal coordinate system deﬁne the

normalized Gauss frame at a point u ∈ U as the coordinate frame with origin at f(u) and

coordinate axes

x(u) =

f

u

(u)

||f

u

(u)||

,y(u) =

f

v

(u)

||f

v

(u)||

,z(u) = g(f(u)).(2.12)

Consider a manifold S with Gauss map g,coordinate patch S

0

,and orthogonal coordinate

system (f,U).The curvature form K at a point s ∈ S

0

is deﬁned as the matrix

K = [ x(u),y(u) ]

T

z

u

(u)

||f

u

(u)||

,

z

u

(u)

||f

v

(u)||

(2.13)

where u = f

−1

(s).The torsion form T at s is the matrix

T = y(u)

T

x

u

(u)

||f

u

(u)||

,

x

v

(u)

||f

v

(u)||

(2.14)

Deﬁne the metric M at s as the diagonal matrix

M =

⎡

⎣

||r

u

|| 0

0 ||r

v

||

⎤

⎦

(2.15)

18 CHAPTER 2.OBJECT MANIPULATION

The metric used is the square root of the Riemannian metric.

Now consider two known contacting objects (O

1

,O

2

) that move while maintaining the

contact with each other.Let C

1

and C

2

be the reference frames of the objects at the contact

point.

Let the z-axis of the C

1

,C

2

frames be normal to the surfaces and outward directed.Let

u

1

= (u

1x

,u

1y

) and u

2

= (u

2x

,u

2y

) be 2D Gauss frame coordinates of the 3D surfaces with

parameterization:(x,y,z) →(u

x

,u

y

).The C

1

,C

2

contact frames of two objects and the 2D

Gauss frames:(u

1x

,u

1y

),(u

2x

,u

2y

) can be seen in ﬁg.2.2.

C

2

C

1

O

1

O

2

x

2

x

1

Ψ

z

1

y

y

2

z

2

1

u

2

u

1

u

1x

1y

u

u

2x

u

2y

Figure 2.2:The C

1

,C

2

contact frames

Let Ψ be the angle between the x-axes of the contact frames of the objects (C

1

,C

2

).Let

M

1

,M

2

be the metric form of O

1

,O

2

respectively,K

1

,K

2

the curvature forms,T

1

,T

2

the

torsion forms.Let ω = (ω

x

,ω

y

,ω

z

) be the relative angular velocity and v = (v

x

,v

y

,v

z

) the

relative linear velocity between the two objects.Let R

Ψ

be the orientation matrix between

C

1

and C

2

(2.16).Let

˜

K

2

be the curvature of O

2

at the point of contact relative to the x-

and y-axes of C

1

(2.17).

ˆ

R

Ψ

=

⎡

⎣

cos Ψ −sinΨ

−sinΨ −cos Ψ

⎤

⎦

(2.16)

˜

K

2

=

ˆ

R

Ψ

K

2

ˆ

R

Ψ

.(2.17)

2.4.MOTION CONSTRAINTS 19

The kinematics of contact between two contacting object are deﬁned as:

˙

u

1

= M

−1

1

(K

1

+

˜

K

2

)

−1

⎛

⎝

⎡

⎣

−ω

y

ω

x

⎤

⎦

−

˜

K

2

⎡

⎣

v

x

v

y

⎤

⎦

⎞

⎠

,(2.18)

˙

u

2

= M

−1

2

R

Ψ

(K

1

+

˜

K

2

)

−1

⎛

⎝

⎡

⎣

−ω

y

ω

x

⎤

⎦

+K

1

⎡

⎣

v

x

v

y

⎤

⎦

⎞

⎠

,(2.19)

˙

Ψ = ω

z

+T

1

M

1

˙

u

1

+T

2

M

2

˙

u

2

,v

z

= 0.(2.20)

We can numerically solve the nonlinear diﬀerential equations (2.18),(2.19),(2.20) for

˙

u

1

,

˙

u

2

and

˙

Ψ if the relative linear and angular velocities on the right side of the equations are

given.These diﬀerential equations are used for both sliding and rolling constraints.Inte-

grating

˙

u

1

,

˙

u

2

and

˙

Ψ we get the position of the contact points in the Gauss frames and the

orientation of C

1

relative to C

2

.Hence a conﬁguration of O

1

with respect to O

2

is deﬁned

by 5 parameters:(u

1x

,u

1y

),(u

2x

,u

2y

) and Ψ.In 2D a conﬁguration of O

1

with respect to O

2

is deﬁned by 2 parameters:u

1x

and u

2x

.

2.4 Motion constraints

The relative velocities between the contacting bodies,and the description of the contacting

surfaces are required to solve the equations of the contact motion.The main contribution of

this chapter is a deﬁnition of a relative velocity generating method for the kinematic model

while the motion of the object and the agents is constrained during the manipulation.The

motion constraints are deﬁned in the following:

• Maintaining contact between the object and the agents - it is given by the

solution of the diﬀerential equations of contact motion (2.18),(2.19),(2.20).

• Equilibrium condition - in case of quasi-static motion the resultant force applied on

the object has to be zero.Any non-grasping forces applied on the object are deﬁned as

“external force”.The grasp transformation is given as G ∈ R

6×6n

if n is the number of

contact points.G transforms forces and torques between the frames of contact points

(C

1

,C

2

,· · ·,C

n

) to the object frame in case of point contact model.It is used to

20 CHAPTER 2.OBJECT MANIPULATION

determine the resultant force applied on the object by the agents.The equilibrium

constraint is written as:

G[f

1

T

,f

2

T

,· · ·,f

n

T

]

T

= −f

O,ext

(2.21)

f

1

,f

2

,· · ·,f

n

denote the contact forces at the contact points’ frames 1,2,· · ·,n,for

instance in 2D f

i

= (f

i

x

,f

i

y

);f

O,ext

is the resultant of the external forces applied on

the object,for example gravity and/or the friction force μ

O

mg between the object and

the plane of motion (μ

O

denotes the coeﬃcient of friction,mthe mass and g coeﬃcient

of gravity).G is the transpose of the resulting Jacobian between object origin and

contact points.

• Rolling constraint - for a pure rolling contact,the relative motion between the agents

and the object is rolling or stationary.The relative linear (v

i

) and angular velocities

(ω

i

) are:

v

i

x

= v

i

y

= v

i

z

= ω

i

z

= 0 (2.22)

where z is the direction of the surface normal.The contact model is point contact

with friction (assuming a Coulomb friction modeling),μ is the friction coeﬃcient.The

contact force has to be inside of the cone of friction.In the motion planning algorithm

this constraint is linearized:instead of cone of friction pyramid of friction is used (ﬁg.

2.3 c).This pyramid has four faces given by

|f

i

x

| < −μf

i

z

/

√

2,|f

i

y

| < −μf

i

z

/

√

2,f

i

z

< 0 (2.23)

The constraint is linearized in order to be able to use algorithm based on linear

programming.

• Sliding constraint - In case of the relative motion is pure sliding,the spatial relative

velocity should satisfy

v

i

z

= ω

i

x

= ω

i

y

= ω

i

z

= 0 (2.24)

The contact force has to be outside of the pyramid of friction (ﬁg.2.3 b).

|f

i

x

| > −μf

i

z

,|f

i

y

| > −μf

i

z

,f

i

z

< 0 (2.25)

2.4.MOTION CONSTRAINTS 21

Figure 2.3:Linearization of friction

The tangential component of the grasping force at a given contact point should lie

in the plane given by v

i

x

and v

i

y

and has to have the same direction and sign (γ is

arbitrary):

f

i

x

= γv

i

x

,f

i

y

= γv

i

y

,γ > 0 (2.26)

• No-collision between any pair of agents - a collision detection algorithm must

be used dealing with the agents,the object,and the obstacles of the environment.It

can be the most time consuming task,depending on the complexity of the surfaces.If

the agents represent hemispherical ﬁngertips of a robotic hand,then the ﬁrst step of

collision detection is to examine the possible collision of the ﬁngertips approximated

by a simple spherical region:

|p

i

−p

j

| ≥ ρ

i

+ρ

j

∀ i,j:i

= j (2.27)

where p

i

and p

j

denote the center of ﬁngertips i and j respectively,ρ is the radius

of the hemisphere.If ﬁngers are used as agents,it is required to check the distance

between any pairs of segments of the ﬁngers (no collision between the ﬁngers).

• Workspace condition - The agents must stay inside their workspace.For example

in case of ﬁngers (dextrous hand) the contact points have to be in the reachable space

of the ﬁngers.Solution of the inverse ﬁnger geometry problem is required.When

the object is a polyhedron,u

1

or u

2

2D contact frame coordinates may be out of

22 CHAPTER 2.OBJECT MANIPULATION

the boundary of the contact face which is interpreted as a transition between two

adjacent faces.Let B,O,F

i

,C

i

,L

i

denote the base frame,the object frame,the i

th

ﬁnger frame,the i

th

contact frame on the object,and the contact frame on the i

th

ﬁnger respectively.The rotational transformation between the i

th

contact frame of the

object and the contact frame of the i

th

ﬁnger is denoted by T

Ψ

i

.For the geometric

problem of the ﬁngers the following homogeneous transformation matrices are used:

– T

B,F

i

between the base coordinate frame and the ﬁngertip frame

– T

B,O

between the base frame and the object frame

– T

O,C

i

between the object frame and the i

th

contact frame on the object

– T

F

i

,L

i

between the ﬁngertip frame and the contact frame on the ﬁngertip

Now the following equation describes the relation between the transformations.

T

B,F

i

= T

B,O

T

O,C

i

T

Ψ

i

T

−1

F

i

,L

i

(2.28)

2.5 Overview of the multi-agent planning system

The planner algorithm consists of two main parts:the global and the local planner (ﬁg.

2.4).The global level plans the object’s motion,while the local planner’s task is to generate

relative velocities and grasping forces for the agents.

Generates relative velocities

Global planner

A*

Local planner, 1st level

Contact forces

Local planner, second level

Tests the paths

Random generation

Kinematics

PLANNER

PLANNER

GLOBAL

LOCAL

Simulated annealing

Figure 2.4:Planning ﬂowchart of the system

2.5.OVERVIEW OF THE MULTI-AGENT PLANNING SYSTEM 23

The task of the global planner is to search for the nominal path in the conﬁguration

space among static obstacles between the initial and the desired position and orientation of

the object (ﬁg.2.5).The global planner does not consider the motion constraints.A graph

search algorithmis used to generate points (subgoals) in the object’s quantized conﬁguration

space,the local planner tries to ﬁnd admissible motion of the agents in contact with the object

between the subgoals.

Figure 2.5:Task of the global planner

In order to simplify notation we introduce the short form T denoting the conﬁguration of

the whole systemin a given moment,T

o

denotes the conﬁguration of the object.Furthermore

prev and curr will be used for ”previous” and ”current” situations,respectively.The local

planner receives T

o,prev

,T

o,curr

and the time step Δt

curr,prev

from the global planner and

begins to ﬁnd the path for the agents and the contact forces.

The global planning starts from T

start

of the object and computes a ﬁnite set of successor

sub-goals using translation and orientation movements of the object with constant angular

and linear velocity for a period of Δt

curr,prev

.No constraints are considered.Since the

orientation in 3D can be represented by 3 scalars,T

o

can be embedded into

6

and T into

6(k+1)

where k is the number of ﬁngers.If the local planner can reach the sub-goal,then

it generates the admissible trajectory and contact forces for the agents leading to T

o,curr

,

otherwise the global planner considers T

o,curr

as forbidden conﬁguration.

24 CHAPTER 2.OBJECT MANIPULATION

The connectivity between the sub-goals is stored incrementally in a graph Γ where the

nodes correspond to conﬁgurations.They may be represented by 6 dimensional adjacent

cells.However,the set of cells is not built by a priori,and only cells visited are generated.

The global level uses the A* graph-search algorithm to ﬁnd a near optimal solution in an

incrementally generated directed graph Γ.A brief description of the A* algorithm and its

properties can be found in Ansari and Hou (1997).A* is admissible,in oder words if there

is a path from the start node to the goal node,A* terminates by ﬁnding the optimal path.

The nodes of the graph are the subgoals in the object’s conﬁguration space,an arc is a

nominal path between two neighboring subgoals.Among the subgoals the best one will be

chosen using a cost function and heuristics.The cost function and the heuristics are based on

Euclidean distance between the set of subgoals reached by the object.The time increment

Δt is chosen so that successive sub-goals lie within the adjacent cells.

There are two main lists in the A* algorithm,the opened and the closed list.The nodes

which were tested before are in the closed list,the successors of the examined nodes are in

the opened list.The lists are denoted by LCT and LT respectively.During the search these

two lists are developed.The best-cost node will be chosen from the opened list and after

generating the successors of the node and updating the cost of those,the node will be placed

on the closed list.LT contains nodes pointed to by arcs corresponding to only nominal paths

(neglecting the constraints),and LCT nodes which have been checked by the local planner

considering the constraints.

The exploration of the graph Γ is iterated until a cell containing the goal conﬁguration

T

o,goal

of the object is reached.Since no explicit presentation of the collection of the cells was

used,checking whether a cell is visited or not is achieved by searching the lists LT and LCT.

Assuming that for a given conﬁguration T

o

of the object Insert(T

o

) creates a node N(T

o

) in

Γ,computes the cost function and inserts N(T

o

) in LT while maintaining this list sorted in

ascending order.

The following information is saved in every node of the search graph:the predecessor of

the node,the distance from the start node (the distance to the goal node is estimated),the

description of the motion sequence of the agents,the position and orientation of the agents

in the node and the object,and ﬁnally the contact forces (the last three are given by the

2.5.OVERVIEW OF THE MULTI-AGENT PLANNING SYSTEM 25

local planner).The pseudo-code of the global planner is shown in ﬁg.2.6.

1 Insert(T

start

,T

o,goal

)

2 WHILE((N(T

goal

) not reached) AND LT

= empty)

3 select first node N(T

o,curr

) from LT

4 LT = LT\N(T

o,curr

)

5 LCT = LCT ∪ N(T

o,curr

)

6 LET N(T

o,prev

) be the node pointing to N(T

o,curr

) in Γ

7 LET Δt

prev,curr

be the time step between T

o,curr

and T

o,prev

8 IF (LOCAL-PLANNER ( T

o,prev

,T

o,curr

,Δt

curr,prev

)

= 0)

9 LET T

o

be the reached configuration of the object

10 IF (T

o

and T

o,goal

in the same cell) GOTO 17

11 FOR( each discrete object nominal velocity V

o

)

12 compute successor T

o,succ

in Γ using V

o

13 IF (successor T

o,succ

not visited) Insert( T

o,succ

,LT )

14 END FOR

15 END IF

16 END WHILE

17 IF (T

o

in the same cell as goal) RETURN( T(t),F(t),t

f

)

18 ELSE RETURN 0

Figure 2.6:Global planner pseudo-code

The detailed trajectories of the agents’ motion (path) between two subgoals are generated

by the local planner.The local planner includes three main parts:

• a) the kinematics solution (computes the trajectories)

• b) the force computing algorithm (solves for the contact forces) and

• c) the generation of the relative velocities between the agents and the object.

The motion sequence generation algorithm starts from the motion of the object,then pro-

duces the motion of the agents.Generating the motion of the object and the motion of the

agents relative to the object,deﬁnes the motion of the whole system.Thus,the motion can

26 CHAPTER 2.OBJECT MANIPULATION

be given by the motion of the object and the relative velocities between the agents and the

object chosen by the local planner algorithm.

The local planner solves a general constrained nonconvex nonlinear program (NP) with

embedded linear program (LP) for contact force determination.The kinematics for relative

motion of agents in the local planner is given by the solution of the diﬀerential equations of

contact motion given the relative velocities.The forces at the contact points are resolved by

the solution of a linear program (LP).The aim of this task is to comply with the constraints

and minimize the contact forces.The constraints of the LP task are the motion constraints

(described in the previous section):the equilibrium constraint and the rolling and sliding

constraints (depending on the relative motion of the agent,described in section 2.4).In

the LP task we minimize the sum of the normal component of the contact forces subject to

the motion constraints (f

i

z

denotes the normal component of the contact force between the

object and the agent number i):

min

f

i

−f

i

z

(2.29)

The advantage of minimizing the normal component of the contact forces is to save in

total energy and to spare the object if it is delicate.Any solution of forces that satisﬁes the

constraints is appropriate because the object’s movement is already deﬁned by the constraints

and the geometry.

An example for another contact force generation method minimizing the instantaneous

power can be found in the work of Cherif and Gupta (1997) and in Vass,Payandeh,and

Lantos (1999c).The minimization of the instantaneous power of the motion is based on the

principle of Peshkin and Sanderson (1989).The instantaneous power of the manipulation

system was minimized subject to the equilibrium and rolling/sliding constraints (v

object

is

the object’s velocity):

min

f

P

v

= v

object

(f

o,ext

+Gf) (2.30)

Minimizing of instantaneous power implies saving in total energy.

In case of quasi-static motion the velocity of the object is assumed to be constant between

two subgoals.If variable object’s velocity is used,the acceleration of the motion changes.If

the acceleration changes linearly then the velocity is a quadratic function of time and hence

the motion is smooth.Sudden jump of the magnitude of forces is not desired.

2.6.RANDOM RELATIVE VELOCITY GENERATION 27

2.6 Random relative velocity generation

The relative motion of the agents on the surface of the object can be only pure rolling,

pure sliding or stationary.In case of 2D application the (x,y) coordinates of the surfaces

of agents and of the object are mapped to a 1D chart (u = (u

x

,0) - Gaussian frame) to

apply the equations of motion (see ﬁg.2.7);in case of 3D application the coordinates of the

surfaces (x,y,z) are mapped to 2D charts (u

x

,u

y

).In ﬁg.2.7 the notation of the 2D cases is

introduced.

Figure 2.7:Reconﬁguration in 2D

The relative velocity between the object and the agents can be chosen independently by

the local planner.In Cherif and Gupta (1997) and Vass,Payandeh,and Lantos (1999c)

random generation is used in a given discretized velocity space in order to decrease the

complexity of search.

A brief review of the random velocity generation algorithm is described in the following.

1.Generate relative velocities for all the agents by choosing from a discretized set of

possible velocities (rolling or sliding) randomly.

2.Execute the motion with these relative velocities.If the subgoal is reached,then it is

considered as reachable,and the relative velocities are stored.Execution is given back

to the global planner.

3.If one or more motion constraint is violated then two cases are distinguished.If the

failure occurred repeatedly more than a given constant,then the subgoal is considered

to be unreachable,execution is given back to the global planner.Otherwise,new

28 CHAPTER 2.OBJECT MANIPULATION

relative velocity is generated randomly for the agent(s) causing the failure.Go back

to step 2.

The random relative velocity generation method works well for relatively simple cases.A

signiﬁcant limitation of the method is that if a relative motion had been already accepted,

then it would never be changed retrospectively in a later moment.If no new relative velocities

fulﬁlling the motion constraints can be found in step 3,then the algorithm does not try to

change the motion from an earlier moment.To cope with this limitation a new method is

introduced in the following sections.

2.7 The V relative velocity matrix

Consider the complete motion sequence of the object and the agents between two subgoals.

The motion of the agents is parameterized by relative linear and angular velocities.In ﬁg.

2.8 the motion of the object and agents is presented,parameterized by the relative velocities

(rolling clockwise and counterclockwise in this example,denoted by - and + respectively,0

denotes stationary relative motion).

Figure 2.8:The motion of the object and agents

The ﬁrst conﬁguration and the object’s position and orientation at the last conﬁguration

are given by the global planner,the in-between frames are ﬁlled up by the local planner.In

the example the object is held by three agents:the orientation for the object and the agents

is marked by radial lines.Forces (direction and magnitude) are marked by arrows.

Let the interval between two subgoals (Δt) be divided into n sub-intervals (δt = Δt/n).

For every δt interval we would like to generate relative velocities for all agents.Consider the

complete motion,there are n ∗ k parameters,where k is the number of agents,hence the

2.8.SIMULATED ANNEALING ALGORITHM AS A LOCAL PLANNER 29

complete motion can be parameterized by a n∗k size matrix of the relative velocities denoted

by V ∈ R

n×k

.The relative velocities should be chosen to comply with the constraints deﬁned

in section 2.2.Let the v

ij

variable be deﬁned as the relative velocity of the i

th

agent in the

j

th

interval.The values of the variables can be quantized (not necessary),for example

v

ij

∈ {0,v

x

max

,−v

x

max

,v

y

max

,−v

y

max

,ω

x

max

,−ω

x

max

,ω

y

max

,−ω

y

max

}.(2.31)

On the left hand side of the ﬁg.2.8 the relative velocity matrix is shown representing

the relative motion of the corresponding agents.Matrix V contains the velocity parameters:

the rows and columns are describing the agents and conﬁgurations respectively.The set of

the applicable relative motions of the agents contains only three elements in this example:

rolling clockwise and counterclockwise,denoted by - and + respectively,0 denotes zero

relative motion.In this motion sequence four time intervals are handled for three agents

(four columns,three rows).

Having the motion of the object and V matrix generated,the motion of the whole system

is deﬁned.In the following section the relative velocity generation method,as a part of the

local planner is described.

2.8 Simulated annealing algorithm as a local planner

In this section,simulated annealing algorithm is applied to search for appropriate relative

velocity parameters.The initial conﬁguration of the agents and the object and the desired

conﬁguration of the object are given as subgoals by the global planner,the task is to compute

the relative velocities to deﬁne the motion of the agents between two subgoals.The simulated

annealing method chooses from a large number of sequences using a relative velocity matrix,

and accepts the best of those (which is near optimal).

The relative velocities v

ij

are chosen automatically by the simulated annealing algorithm

which tries to optimize the n ∗ k size matrix of the relative velocities considering given

constraints (shown in section 2.4).

An example of generation of the velocity matrix V is presented in (ﬁg.2.9).The local

planner chooses fromdiﬀerent motion sequences which were generated fromdiﬀerent velocity

matrices.The decision is based on the energy function.

30 CHAPTER 2.OBJECT MANIPULATION

!

INPUT: 2 SUBGOALS

SEARCH FOR THE TRAJECTORIES

?

ENERGY 1

ENERGY 2

ENERGY 3

Figure 2.9:Generation of the velocity matrix V

In the approach used for the relative velocity generation the energy function of the simu-

lated annealing algorithm can be a combination of analytical (for example:equilibrium and

no collision) and heuristic expressions (for example:small contact forces).For example,if

the energy function is deﬁned as the number of collisions during the motion then minimizing

the energy function will minimize the number of collisions.

The energy function of the simulated annealing algorithm has several components (e.g.

(2.32)):the ﬁrst and the second components are deﬁned to be:

• a) Force equilibrium (applied on the object),

• b) No collision between agents.

Other components can be heuristics,for example:

• c) the absolute sum of contact forces for all agents in all δt intervals has to be

small.

This is useful if the object is delicate or if the objective is to minimize the energy.Given

a conﬁguration of the agents and the object,(2.29) gives the minimal average contact forces.

2.8.SIMULATED ANNEALING ALGORITHM AS A LOCAL PLANNER 31

The simulated annealing algorithm in this case chooses from diﬀerent conﬁgurations,pre-

ferring that conﬁguration which has the minimal absolute sum of contact forces.Another

component can

• d) minimize the number of relative velocity changes.

A typical energy function (objective function,cost function,performance index) can be

written as:

E(V) = K

c

n

j =1

c

j

+K

e

n

j =1

e

j

+K

f

k

i=1

n

j =1

|f

ij

| +K

g

k

i=1

g

i

(2.32)

The ﬁrst sum of (2.32) is the penalty due to collision (the number of collisions multiplied

by a K

c

constant),the second is the penalty due to no force equilibrium.The third part is

the absolute sum of the contact forces,the fourth is the number of velocity changes of the

agents.The deﬁnitions of the variables and constants are the following:

• E is the energy function to be minimized.

• V contains the relative velocities,the actual parameters which describe the motion of

the agents relative to the object.

• K

c

,K

e

,K

f

,K

g

are positive weight constants for collision,equilibrium,forces and

velocity changes respectively.K

c

≈ K

e

,K

c

,K

e

K

f

,K

g

because no collision and

equilibrium are more important than small contact forces and small number of velocity

changes.

• n and k denote number of intervals between two subgoals and the number of agents

respectively.

• c

j

=

⎧

⎨

⎩

0 if there is no collision in the j

th

interval

1 otherwise

• e

j

=

⎧

⎨

⎩

0 if there is force equilibrium in the j

th

interval

1 otherwise

• f

ij

is the contact force applied by the i

th

agent at the j

th

interval.

• g

i

is the number of relative velocity change of the i

th

agent between the two subgoals.

32 CHAPTER 2.OBJECT MANIPULATION

The force component of the energy function uses (2.29).This force computing sub-

algorithm (LP) is called for every conﬁguration in every iteration.The algorithm prefers

those motion sequences which have the smaller sumof contact forces,less number of collisions

and velocity changes and more conﬁgurations with equilibrium.

The pseudo-code of the relative velocity computing algorithm is shown in ﬁg.2.10.

1 Get 2 subgoals (start,goal)

2 Generate random relative velocities(V)

3 E

old

= Compute

energy(V)

4 WHILE (T > T

end

) DO

5 Select (i,j) randomly

6 Generate a random velocity (v

rnd

)

7 E

new

=Compute

energy(V,v

ij

= v

rnd

)

8 ΔE = E

new

−E

old

9 IF E

new

< E

old

THEN

10 LET v

ij

= v

rnd

,E

old

= E

new

11 ELSE IF e

−ΔE/T

> Random(1) THEN

12 LET v

ij

:= v

rnd

13 END IF

14 Decrease(T)

15 END WHILE

Figure 2.10:Local planner pseudo-code

In every cycle of the simulated annealing algorithm the path between the two subgoals

has to be generated with the selected relative velocities then the energy function has to be

computed.The number of parameters (relative velocities) is directly proportional to the

number of agents and the number of intervals between two subgoals.

Comments on local planner pseudo-code in ﬁg.2.10:

1 Two subgoals are given by the global planner.

2 n*k initial relative velocities are generated (for n intervals and k ﬁngers).

2.9.APPLICATION FOR ROBOTIC MANIPULATORS 33

3 Compute the complete motion and the energy for the complete path between the two

subgoals with the initial values of the relative velocities.

4 T is the temperature.Do the cycle until the desired ﬁnal temperature T

end

is reached.

5 Choose a variable from matrix V.

6 Random velocity generation,for example x = v

max

.

7 Generate the new positions for all agents for all the intervals and compute the energy

function with the new value of the selected variable v

ij

= x.

8 The change of the energy function due to the change of the selected variable.

9 Test the energy function.

10 If the change of energy is positive then the new state is accepted.

11 function Random(n) generates a random number between 0 and n.

12 If the change is negative,then do not apply the new state.

14 Decrease temperature according to the temperature schedule (see Appendix).

In case the simulated annealing method cannot ﬁnd any motion sequence without any

collision or without force equilibrium through the whole motion,the second subgoal will be

considered as unreachable from the ﬁrst subgoal.In ﬁg.2.11 a simulated 3D example is

shown for the relative motion.The agents were represented by the three smaller spheres.

2.9 Application for robotic manipulators

In this section two possible applications of the object reconﬁguration algorithmare presented.

In the ﬁrst case three manipulators each having six degrees of freedom (DOF) use the

algorithm (multiple arm planning system),while in the more diﬃcult second setup a single

six degrees of freedom robotic arm and a three ﬁngered hand is used (multi-ﬁnger planning

system).

2.9.1 Cooperating multiple robotic arms

In the ﬁrst case the algorithm can be applied to multiple 6-DOF robotic arms in 3D.The

motion of the end eﬀectors relative to the object can be sequences consisting of pure sliding

34 CHAPTER 2.OBJECT MANIPULATION

Figure 2.11:Motion sequence of 3D agents

and pure rolling.In order to demonstrate the applicability of the developed method to

the design of object reconﬁguration with mobile agents including cooperating robots,in the

simulation three manipulators were implemented each having six degree of freedom.The

model of each manipulator satisﬁes the joint formula and Denavit-Hartenberg form of the

PUMA 560 robotic arm (see ﬁg.2.12).

2.9.2 Puma robot with dextrous hand

The second application of the algorithm is an object manipulating system for the 6-DOF

PUMA 560 arm with a three ﬁngered TUB-PC hand,each ﬁnger having three 3 DOF.

Detailed formulas can be found in Soml´o,Lantos,and Cat (1997) for direct and inverse

geometry and Jacobian matrix of the PUMA 560 arm (Chapter 2),and in Ludvig (1997) for

direct and inverse geometry (Chapter 4) and ﬁnger Jacobian (Appendix D) of the TUB-PC

hand.The whole system has 15 DOF,which yields boundaries in the applicability of the

## Σχόλια 0

Συνδεθείτε για να κοινοποιήσετε σχόλιο