OBJECT MANIPULATION PLANNING FOR DEXTROUS ROBOT SYSTEMS

chestpeeverAI and Robotics

Nov 13, 2013 (4 years and 1 month ago)

302 views

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 Fulfillment 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
Office 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 influenced my work on this thesis,provided me
with scientific 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 multifingered hands.Three meth-
ods have been developed for object manipulation design and real-time trajectory planning.
The first result is a new object manipulation algorithmusing artificial 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 reconfigure
the grasp.The benefit of the method is more stable manipulation and more flexibility 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.Modification
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 ´atkonfigur´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 figyelembe 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 flowchart of the system........................22
2.5 Task of the global planner............................23
2.6 Global planner pseudo-code...........................25
2.7 Reconfiguration 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 different 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 first,left to right.....44
3.1 Object reconfiguration with three fingers 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 flowchart 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 first,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 profiles 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
finger frame
F
p
the frame of the pseudo-agent
γ jerk (first 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
finger
μ coefficient 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 fingertip frame
T
B,O
transformation between the base frame and the object frame
T
F
i
,L
i
transformation between the i
th
fingertip frame and the finger 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
fingertip
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 difficult problem.First collision free paths
for all fingers 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 fingertips 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-configuration
problem) is stated as the following:given an initial grasp of the object find the motion’s
trajectories of the fingertips to move the object to the desired configuration.The forces
exerted at the contact points by the fingertips are determined first 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
classified 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 fingertips 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,stiffness 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 fingertips are used to demonstrate the concept of finger
gaiting and rolling.The group of fingers is partitioned during the manipulation into grasping
fingers and free fingers.Two finger gaiting primitives are introduced:finger rewind limiting
fingers back to their workspace and finger substitution to remove the limiting finger from
the grasp.
A wide survey of the field of artificial 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 field 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 configuration.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 field methods,multiple moving objects,dealing with uncertainty and movable
objects.
Recently,in the field of object manipulation a relatively new concept of stratified control
has appeared.A representative class of stratified control applications are the manipulation
systems.Harmati,Lantos,and Payandeh (2000a) provide a basic concept for object manip-
ulation with finger relocation using stratified control approach for nonlinear systems where
configuration 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 configuration
without considering the contact configuration;
• the grasp adjustment attains the desired contact configuration by disregarding the
object configuration;
3
• dextrous manipulation leads the robotic hand to its final state taking into account the
desired object and contacts configuration,respectively.
Harmati,Lantos,and Payandeh (2005) concerns with object and dextrous manipulation,
proposes two methods to lead the object into a desired configuration while the fingertips
reach the desired contact points on the surface.Both of them rely on stratified 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
fingertips but finger relocation is allowed.
Force closure stability of the grasping is guaranteed by sufficient number of fingers.In the
paper an example of dextrous manipulation is shown for a four fingered robotic hand and an
object of smooth surface.An overview is given about the stratified motion planning,which
is based on a smooth motion planning and operates on sequences of flows.The solution of
the smooth and stratified motion planning problem consists of the sequence of flows along
the vector fields of the system.The described manipulation algorithms are based on a simple
fictitious (fitted) system that reduces the complexity of the computations.Hence,they make
the interpretation of the state trajectories easy from the results in the configuration space.
The first proposed method is based on the philosophy of the stratified motion planning and
uses a special fictitious systemcalled fitted system.The special parameterization of the fitted
system yields simple vector fields where one also can easily check any system’s property (e.g.
stratified controllability).The method is able to carry out a dexterous manipulation in the
restricted workspace of the fingertips.However,the method does not assure automatically
the force closure stability and there is no protection against finger collisions.
The second method is a semi-stratified motion planning on a fitted system using task
decomposition Harmati,Lantos,and Payandeh (2000b).Beside the stratified motion plan-
ning,the semi-stratified motion planning includes a strategy for systematic finger relocations.
The strategy is based on a subsegment generation procedure providing reference fingertip
positions to a desired object motion,which can be chosen suitably.Thus,the method has
restrictions,arbitrary fingertip 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 finger relocation than the first method because it allows any trajectory in the free
space for the fingertips.The simulation results are demonstrated through a manipulation
example of an egg by four fingers.
Adifferent 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 flatness property of the system simplifies the parametrization of feasible trajectories.
A system is differentially flat,if one can find a set of differentially independent variables
(called the flat output) such that every variable in the system (including the inputs) are
functions of the flat output and a finite number of its time derivatives.The flat output is a
function of state variables,the input variables and the input derivatives up to a finite order.
The flat output has the same number of components as the input vector.As a consequence of
the definition of the flat 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 flatness 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 finger.The key of the planning problem is
to find 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 multifingered 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 fitted any PUMA or SCARA type robots.
Figure 1.1:TUB-PC Hand
The TUB-PC hand is equipped with three fingers,arranged symmetrically on a pla-
nar palm.Fingers are identical,each finger 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 fitted 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 finished by 1997.
The control system is divided into subsystems.The first 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 different 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 amplifiers of the driving motors of the tendons.The servo amplifiers of the
motors are removed to a separate rack.Each finger 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 fingers into
the position angles of the motors and the coordinated position control of the motors driving
the tendons (4 motors are driving 3 finger 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
fingers and the arm are needed,the joint coordinates are computed real time from the
Cartesian coordinates of the fingertips.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 fingertips.
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 fingers (one for each finger),the hand control law,and the informa-
tion exchange between robot and hand controllers.The remaining process reads the off-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 fingertips
• 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-
fingered 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 configuration of the object.In
this approach,fingertips 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 reconfiguration with
multiple agents including cooperating robots.Only pure rolling and pure sliding relative
motions between the fingertips and the object and finger relocation are assumed.The planner
allows breaking contact between a fingertip and the object due to finger 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 finger 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-configuration 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 different position and orientation with a robotic hand.
Chapter 3 describes two extensions for the object reconfiguration method.The first
contribution is a method for contact point relocation,which allows the manipulating agent
to break contact and later find 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 flexibility 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 satisfies the motion constraints.
Chapter 4 contains a trajectory planning algorithmwhich takes into account the maximum
jerk (the first 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 modifies the motion trajectories generated by the MSRS
approach in order to decrease the time required for the robot motion.Modification of the
spatial trajectory on the fly 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 definitions 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 field in
robotic research.In this chapter a motion planner for object reconfiguration with multiple
dextrous agents is proposed.
Object manipulation first 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 reconfiguration
problem) is stated as the following:given an initial grasp of the object find the motion’s
trajectories of the agents to move the object to the desired configuration.In general collision
free paths for all agents must be found toward the contact points on the object (pre-grasp
configuration) and the grasping and manipulation forces should then be exerted on the object
by the agents.These forces are determined first 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 configuration space among static obstacles between the initial and the
desired position and orientation of the object by generating points (subgoals) in the object’s
configuration space.The local planner tries to find 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-finger 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 classified 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 (fig.2.1).The two relative motion types have difference in the exerted
forces in the contact points (denoted by arrows in the figure):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 defining 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 find a series of near time-optimal final con-
figurations for two arms where the inspection operations are undertaken for segment (link)
motions and find 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 find a near time-optimal route.
In Lee and Kardaras (1997) an efficient 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
field.Simulated annealing approach is used for local minima problems from the potential
field.The potential field 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 configuration space (C-space).The expert
system performs geometric reasoning on the C-space so that only a small fraction of config-
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 finger tracking.It is
used for in-hand manipulation of rigid,piecewise smooth 3-D objects.The object manipu-
lation problem when only the fingers and the object move,but not the arm is called in-hand
manipulation.The basic idea of the method is that the fingers cooperate to reorient the
object by applying a normal force while sliding compliantly on the object surface.The dif-
ferential control for finger tracking is described and analyzed,and extended to a continuous
control for a set of cooperating robot fingers.
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 finger
relocation hence we use first 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
first 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 multifingered hands is presented
by Zheng,Nakashima,and Yoshikawa (2000).The static and dynamic frictions between
the sliding finger and the surface of the object are considered and their effects 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 define 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 finally 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
fingertip
of a robotic hand.Let B,O,F
i
,C
i
,L
i
denote the base frame,the object frame,the i
th
finger frame,the i
th
contact frame on the object,and the contact frame on the i
th
finger
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 fingertip 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 defines the point,
of which linear and angular velocities are considered.The top-left index identifies 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 fix B base frame.The relative
linear and angular velocities v
i
and ω
i
between the i
th
finger 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
finger 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
finger 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 define 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 affects 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,define 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 define 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 defined 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)
Define 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 fig.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 defined 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 differential 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 differential 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 configuration of O
1
with respect to O
2
is defined
by 5 parameters:(u
1x
,u
1y
),(u
2x
,u
2y
) and Ψ.In 2D a configuration of O
1
with respect to O
2
is defined 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 definition 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 defined in the following:
• Maintaining contact between the object and the agents - it is given by the
solution of the differential 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 defined 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 coefficient of friction,mthe mass and g coefficient
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 coefficient.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 (fig.
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 (fig.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 fingertips of a robotic hand,then the first step of
collision detection is to examine the possible collision of the fingertips 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 fingertips i and j respectively,ρ is the radius
of the hemisphere.If fingers are used as agents,it is required to check the distance
between any pairs of segments of the fingers (no collision between the fingers).
• Workspace condition - The agents must stay inside their workspace.For example
in case of fingers (dextrous hand) the contact points have to be in the reachable space
of the fingers.Solution of the inverse finger 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
finger frame,the i
th
contact frame on the object,and the contact frame on the i
th
finger respectively.The rotational transformation between the i
th
contact frame of the
object and the contact frame of the i
th
finger is denoted by T
Ψ
i
.For the geometric
problem of the fingers the following homogeneous transformation matrices are used:
– T
B,F
i
between the base coordinate frame and the fingertip 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 fingertip frame and the contact frame on the fingertip
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 (fig.
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 flowchart 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 configuration
space among static obstacles between the initial and the desired position and orientation of
the object (fig.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 configuration
space,the local planner tries to find 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 configuration of
the whole systemin a given moment,T
o
denotes the configuration 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 find the path for the agents and the contact forces.
The global planning starts from T
start
of the object and computes a finite 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 fingers.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 configuration.
24 CHAPTER 2.OBJECT MANIPULATION
The connectivity between the sub-goals is stored incrementally in a graph Γ where the
nodes correspond to configurations.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 find 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 finding the optimal path.
The nodes of the graph are the subgoals in the object’s configuration 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 configuration
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 configuration 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 finally 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 fig.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,defines 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 differential 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 satisfies the
constraints is appropriate because the object’s movement is already defined 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 fig.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 fig.2.7 the notation of the 2D cases is
introduced.
Figure 2.7:Reconfiguration 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
significant 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
fulfilling 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 fig.
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 first configuration and the object’s position and orientation at the last configuration
are given by the global planner,the in-between frames are filled 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 defined
in section 2.2.Let the v
ij
variable be defined 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 fig.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 configurations 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 defined.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 configuration of the agents and the object and the desired
configuration of the object are given as subgoals by the global planner,the task is to compute
the relative velocities to define 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 (fig.2.9).The local
planner chooses fromdifferent motion sequences which were generated fromdifferent 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 defined 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 first and the second components are defined 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 configuration 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 different configurations,pre-
ferring that configuration 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 first 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 definitions 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 configuration 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 configurations with equilibrium.
The pseudo-code of the relative velocity computing algorithm is shown in fig.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 fig.2.10:
1 Two subgoals are given by the global planner.
2 n*k initial relative velocities are generated (for n intervals and k fingers).
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 final 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 find any motion sequence without any
collision or without force equilibrium through the whole motion,the second subgoal will be
considered as unreachable from the first subgoal.In fig.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 reconfiguration algorithmare presented.
In the first case three manipulators each having six degrees of freedom (DOF) use the
algorithm (multiple arm planning system),while in the more difficult second setup a single
six degrees of freedom robotic arm and a three fingered hand is used (multi-finger planning
system).
2.9.1 Cooperating multiple robotic arms
In the first case the algorithm can be applied to multiple 6-DOF robotic arms in 3D.The
motion of the end effectors 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 reconfiguration with mobile agents including cooperating robots,in the
simulation three manipulators were implemented each having six degree of freedom.The
model of each manipulator satisfies the joint formula and Denavit-Hartenberg form of the
PUMA 560 robotic arm (see fig.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 fingered TUB-PC hand,each finger 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 finger Jacobian (Appendix D) of the TUB-PC
hand.The whole system has 15 DOF,which yields boundaries in the applicability of the