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 1255597 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 1255597,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 realtime 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 pseudoagent,the real agents can relocate the current contact points resulting in a
better position for the further manipulation.
The third result is a realtime method for motion planning with jerk,improving the
Minimumtime Splinebased 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 realtime 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 realtime.The
optimized algorithm has been realized under RTOS32 realtime 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˝ooptimaliz´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 RTOS32 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 multiagent 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 pseudoagent 46
vi
3.1 Introduction....................................46
3.2 Contact point relocation.............................50
3.3 Static objects as pseudoagents.........................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 TUBPC 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 pseudocode...........................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 pseudocode............................32
2.11 Motion sequence of 3D agents..........................34
2.12 Three robotic manipulators grasping an object.................35
2.13 PUMA 560 arm with TUBPC 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 pseudoagent.................54
3.6 System ﬂowchart for handling pseudoagents..................55
3.7 Start contact with the pseudoagent (obstacle).................57
3.8 Usage of pseudoagent in 3D...........................58
3.9 Object manipulation applying a pseudoagent.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
#
pseudoinverse 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 pseudoagent
γ 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 reconﬁ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 semistratiﬁed motion planning on a ﬁtted system using task
decomposition Harmati,Lantos,and Payandeh (2000b).Beside the stratiﬁed motion plan
ning,the semistratiﬁ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 onepoint 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 handobject
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 realtime
systems.
In the article it is proven that the ﬂatness property holds to the entire handobject system
if one can integrate the nonholonomic constraints.An example of a planar handobject
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 TUBPC hand is rather universal,
therefore the hand can be ﬁtted any PUMA or SCARA type robots.
Figure 1.1:TUBPC Hand
The TUBPC hand is equipped with three ﬁngers,arranged symmetrically on a pla
nar palm.Fingers are identical,each ﬁnger has four degreesoffreedom according to the
TRR⊥R joint formula.The rotational joints are tendon operated,driven by miniature
DC motorplanetary 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 tachoprocessor 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
CANbus.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 realtime operation system.QNX is a Posixlike,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,selftuning 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 tendondriven TUBdextrous hand,developed by CEIT,
consists of a PC with extension rack containing an analog input card (PCL818 with 16
inputs) for the analog position signals and an analog output card (PCL727 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 lowlevel 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 positionforce 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 pathplanning 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 TUBPC 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 realtime 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 quasistatic 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 timeoptimal 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
reconﬁ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 pseudoagents.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 timeoptimal 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 Minimumtime Splinebased 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 realtime.The algorithm
is fast enough to use in realtime 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 (pregrasp
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 subproblems: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 generalpurpose 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 softcomputing in robotics.Simulated an
nealing is frequently applied on the strategic level of motion planning,for example to comply
with task constraints.A near timeoptimal inspectiontasksequence 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 tasksequence planning is to ﬁnd a series of near timeoptimal ﬁnal con
ﬁgurations for two arms where the inspection operations are undertaken for segment (link)
motions and ﬁnd a near timeoptimal task sequence of inspection points.The tasksequence
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 timeoptimal 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 multilayered 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 (Cspace).The expert
system performs geometric reasoning on the Cspace 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 online 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 inhand manipulation of rigid,piecewise smooth 3D objects.The object manipu
lation problem when only the ﬁngers and the object move,but not the arm is called inhand
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 3D rolling contacts in twoarm 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 threedimensional rolling contacts are
derived.A nonlinear feedback control algorithm that decouples and linearizes the system is
shown.The dynamic control of 3D 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 quasistatic 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 zaxes 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 basisdependent 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 bottomright index deﬁnes the point,
of which linear and angular velocities are considered.The topleft index identiﬁes the basis
vectors.If it is apparent from the environment,then the topleft 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 basisindependent form:
f
O
= f
C
i
m
O
= m
C
i
+p
O,C
i
×f
C
i
(2.9)
In basisdependent 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 quasistatic 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 twomanifold 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 zaxis 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 xaxes 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 yaxes 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 quasistatic motion the resultant force applied on
the object has to be zero.Any nongrasping 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)
• Nocollision 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 multiagent 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 MULTIAGENT 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
subgoals 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 subgoal,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 subgoals 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* graphsearch 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 subgoals 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 bestcost 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 MULTIAGENT PLANNING SYSTEM 25
local planner).The pseudocode 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 (LOCALPLANNER ( 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 pseudocode
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 quasistatic 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 inbetween 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 subintervals (δ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 pseudocode 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 pseudocode
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 pseudocode 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 6DOF 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 DenavitHartenberg 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 6DOF
PUMA 560 arm with a three ﬁngered TUBPC 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 TUBPC
hand.The whole system has 15 DOF,which yields boundaries in the applicability of the
Enter the password to open this PDF file:
File name:

File size:

Title:

Author:

Subject:

Keywords:

Creation Date:

Modification Date:

Creator:

PDF Producer:

PDF Version:

Page Count:

Preparing document for printing…
0%
Commentaires 0
Connectezvous pour poster un commentaire