University of Pennsylvania ScholarlyCommons

IRCS Technical Reports Series Institute for Research in Cognitive Science

December 1996

Continuous Methods for Motion Planning

Milos Zefran University of Pennsylvania

Follow this and additional works at: https://repository.upenn.edu/ircs_reports

Zefran, Milos, "Continuous Methods for Motion Planning" (1996). IRCS Technical Reports Series. 111. https://repository.upenn.edu/ircs_reports/111

University of Pennsylvania Institute for Research in Cognitive Science Technical Report No. IRCS-96-34.

This paper is posted at ScholarlyCommons. https://repository.upenn.edu/ircs_reports/111 For more information, please contact [email protected]. Continuous Methods for Motion Planning

Abstract Motion planning for a robotic system addresses the problem of finding trajectory and actuator forces that are consistent with a given set of constraints and perform a desired task. In general, the problem is under determined and admits a large number of solutions. The main claim of this dissertation is that a natural way to resolve the indeterminacy is to define performance of a motion and find a solution with the best performance. The motion planning problem is thus formulated as a variational problem. The proposed approach is continuous in the sense that the motion planning problem is not discretized.

A distinction is made between dynamic and kinematic motion planning. Dynamic motion planning provides the actuator forces as part of the motion plan and requires finding a motion that is consistent with the dynamic equations of the system, satisfies a given set of equality and inequality constraints, and minimizes a chosen cost functional. In kinematic motion planning, dynamic equations of the system are not taken into account and it is therefore simpler.

For dynamic motion planning, a novel numerical method for solving variational problems is developed. The continuous problem is discretized by finite element methods and techniques from nonlinear programming are used to solve the resulting finite dimensional optimization problem. The method is employed to find smooth trajectories and actuator forces for two planar cooperating manipulators holding an object. The computed trajectories are used to model human motions in a two-arm manipulation task. The method is then extended for systems that change the dynamic equations as they move. An example of a simple grasping task illustrates that for such systems variational approach unifies motion planning and task planning.

Kinematic motion planning is formulated as a variational problem on the group of spatial rigid body displacements, SE(3). A Riemannian metric and an affine connection ear introduced to define cost functionals that measure smoothness of trajectories. Metrics and connections that are important for kinematic analysis are identified. It is then shown how the group structure of SE (3) can be used to find smooth trajectories that satisfy boundary conditions on positions, orientations, velocities or their derivatives, and have certain invariance properties with respect to the choice of the inertial and body fixed frames.

Comments University of Pennsylvania Institute for Research in Cognitive Science Technical Report No. IRCS-96-34.

This thesis or dissertation is available at ScholarlyCommons: https://repository.upenn.edu/ircs_reports/111 , Institute for Research in Cognitive Science

Continuous Methods for Motion Planning

Milos Zefran

University of Pennsylvania 3401 Walnut Street, Suite 400A Philadelphia, PA 19104-6228

December 1996

Site of the NSF Science and Technology Center for Research in Cognitive Science

IRCS Report 96--34

CONTINUOUS METHODS FOR MOTION PLANNING

Milos Zefran

A Dissertation

in

Computer and Information Science

Presented to the Faculties of the UniversityofPennsylvania in Partial Fulllment of the

Requirements for the Degree of Do ctor of Philosophy

Prof Vijay Kumar

Sup ervisor of Dissertation

Prof Mark Steedman Graduate Group Chairp erson

Mo jima starsema Nadi in Srecku ter bratu Rastku ii

Acknowledgment

It is dicult to mention all the p eople who come to mind when I lo ok up on the path

that led to this work Ab oveallIwould like to express my admiration and gratitude to

Professor Vijay Kumar for b eing my advisor and such an inspiring research collab orator

He never hesitated to work with me on a lastminute pap er submission or to patiently listen

when I tried to explain my often confused ideas And he was a fun conference companion

I am equally indebted to Professor Chris Croke He intro duced me to dierential

geometry and was prepared to get involved into researchthatwas quite marginal to his

researchinterests Collab oration with him was one of the most satisfactory exp eriences in

my graduate scho ol

Iwould also like to thank the memb ers of my committee Professors Ruzena Ba jcsy

Max Mintz Richard Murray and Xiaoping Yun for reading through the dissertation

and making a numb er of helpful suggestions Further I am grateful to Professor Ba jcsy

director of the GRASP Lab for bringing me into the lab oratory and for maintaining such

a unique environment there I thank Professor Mintz for many pleasantchats b e they

ab out research or other things Professor Murray kindly answered questions in my mails

and Professor Yun intro duced me to the research in the GRASP Lab when I rst came

there

In the last four years GRASP Lab was my second home and I would like to thank all

of its current and past denizens for making it such a vibrant place Camaraderie of Luca

Bogoni Ulf Cahn von Seelen HanyFarid Ioannis Kakadiaris Jana Kosec ka and the rest

of the crowd in and out of the Lab made these years the exp erience of a lifetime I assure

everyb o dy in the Lab that I did not work late at night to hide from them Brian Madden

was an entertaining ocemate Venkat Krovi was always ready for a discussion and kept

me in touch with the so cial life at the University Stamps Howard and Jaydev Desai were

great coworkers and Nilanjan Sarkar Yoshio Yamamoto and ChauChang Wang gaveme

many wise advices ab out research

These years in Philadelphia were very sp ecial thanks to Barbara Di Eugenio Her at

tention and patience kept me going when I struggled with the research And the wonderful

trips we to ok together were the b est imaginable way of sp ending the free time She also

kindly corrected many grammatical mistakes in the text

I am grateful to my ro ommate Evangelos Kokkevis for his friendship and for reminding

me that there is life b eyond scho ol I appreciate his willingness to put up with my co oking

And I thank him for his help with the computer animations

When I rst started my research back at the University of Ljubljana I found a wonderful

mentor and friend in Professor Tadej Ba jd I thank him for all he has done for me iii

I gratefully acknowledge the Institute for Research in Cognitive Science at the Univer

sityofPennsylvania for providing most of my funding during graduate scho ol

And nallyIwould like to thank myparents my brother and his family for their

encouragement and supp ort during my long absence from home This work is dedicated

to them iv

Abstract

CONTINUOUS METHODS FOR MOTION PLANNING

Milos Zefran

Vijay Kumar

Motion planning for a rob otic system addresses the problem of nding a tra jectory and

actuator forces that are consistent with a given set of constraints and p erform a desired

task In general the problem is underdetermined and admits a large numb er of solutions

The main claim of this dissertation is that a natural way to resolve the indeterminacy is to

dene p erformance of a motion and nd a solution with the b est p erformance The motion

planning problem is thus formulated as a variational problem The prop osed approachis

continuous in the sense that the motion planning problem is not discretized

A distinction is made b etween dynamic and kinematic motion planning Dynamic

motion planning provides the actuator forces as part of the motion plan and requires

nding a motion that is consistent with the dynamic equations of the system satises a

given set of equality and inequality constraints and minimizes a chosen cost functional In

kinematic motion planning dynamic equations of the system are not taken into account

and it is therefore simpler

For dynamic motion planning a novel numerical metho d for solving variational prob

lems is develop ed The continuous problem is discretized by niteelement metho ds and

techniques from nonlinear programming are used to solve the resulting nitedimensional

optimization problem The metho d is employed to nd smo oth tra jectories and actuator

forces for two planar co op erating manipulators holding an ob ject The computed tra jecto

ries are used to mo del human motions in a twoarm manipulation task The metho d is then

extended for systems that change the dynamic equations as they move An example of a

simple grasping task illustrates that for such systems variational approach unies motion

planning and task planning

Kinematic motion planning is formulated as a variational problem on the group of

spatial rigid b o dy displacements SE A Riemannian metric and an ane connection

are intro duced to dene cost functionals that measure smo othness of tra jectories Metrics

and connections that are imp ortant for kinematic analysis are identied It is then shown

how the group structure of SE can b e used to nd smo oth tra jectories that satisfy

b oundary conditions on p ositions orientations velo cities or their derivatives and have

certain invariance prop erties with resp ect to the choice of the inertial and b o dy xed

frames v

Contents

Acknowledgment iii

Intro duction

Motion planning in humans

Tra jectory generation in rob otics

Implicit schemes

Explicit schemes

Continuous motion planning

Approach in the dissertation

Contents of the dissertation

Relation b etween optimal control and calculus of variations

Optimal control problem and Pontryagins minimum principle

Variational reformulation of the optimal control problem

Equivalence of the extremals

Regular and singular extremals

Numerical metho d

Numerical metho ds for solving optimal control problems

Firstorder metho d for nding the extremals

State dep endent equality constraints

State dep endent inequality constraints

A direct metho d for solving the Bolza problem

Penalty functions and augmented Lagrangian

Minimization by Newtons metho d

Discretization

Computation of the gradient rH

c

Computation of the Hessian r H

c

Discussion

Choice of the cost functional

Motion planning for two cooperating robots

Dynamic mo del for two manipulators holding an ob ject

Motion planning for two planar co op erating arms

No constraints on the interaction forces

Frictional constraints on the interaction forces vi

Numerical results

Discussion

Mo deling of human twoarm motion planning

Exp erimental results

General observations

Motions in the sagittal plane

Other motions

Optimal forces and tra jectories predicted by the mo del

Discussion

Motion planning for systems with discrete and continuous state

Problem formulation

Metho d for computing switching times

Illustrative example

Mathematical formulation

Simulation results

Discussion

Task space and the sp ecial Euclidean group SE

Kinematics Lie groups and dierential geometry

Left invariantvector elds

Exp onential map and lo cal co ordinates

Riemannian metrics on Lie groups

Ane connection and covariantderivative

Geo desics

Metrics and screw motions

Screw motions as geo desics

Invariance of the family of semiRiemannian metrics

Geo desics of the family of semiRiemannian metrics

Ane connections on SE

Kinematic connection

Metrics compatible with the kinematic connection

Discussion

Task space tra jectory planning

Choice of metric on SE for motion planning

Riemannian connection on SE

Curvature of SE

Acceleration and jerk of rigid b o dy motions in IR

Necessary conditions for smo oth tra jectories

Variational calculus on manifolds

Minimumdistance curves geo desics

Minimumacceleration curves

Minimumjerk curves

Minimum energy curves vii

Analytical expressions for optimal tra jectories

Shortest distance path on SE

Minimumacceleration and minimumjerk tra jectories

Comparison b etween kinematic and dynamic motion planning

Discussion

Concluding remarks

Summary

Contributions

Possible applications and future work

A Pro duct metric in the basis fL g

i

B Pro ofs

B Pro of of Prop osition

B Pro of of Theorem

B Pro of of Theorem

C Metric with screw motions as geo desics

D Metric compatible with the acceleration connection

E Lie brackets for se

F The logarithm function on SO and SE viii

List of Figures

Spaces in which motion can b e observed and mappings b etween them

Division of the approaches to motion planning found in the literature

Shap e functions used in the numerical metho d

Approximation of the state a and its derivative b

Internal force in an overactuated system

Two planar R arms holding an ob ject

Tra jectories of the center of mass of the ob ject

Optimal distribution of the torques for the preload force of N

Ratio of the tangential vs normal force for the preload force of N

Ratio of the tangential vs normal force for the preload force of N

Exp erimental setup

Sample plots of normalized velo city proles measured during the exp eriments

Sample plots of the measured forces a Total force acting on the ob ject b

Internal forces in the axial direction

Motions in the sagittal plane a Tra jectories for inward solid and outward

dotted motions b Internal forces in the axial direction

y prole Comparison of the measured velo city proles solid with the velo cit

predicted by the mo del dashed

Comparison of the measured forces solid with the predictions of the mo del

dashed a Leftarm forces b Rightarm forces

Aschematic of a system with changing dynamic b ehavior

A new indep endentvariable has xed values at the switching times t

i

Two ngers rotating a circular ob ject in a plane

a Angles and and b velo cities and for the workspace

a Angles and and b velo cities and for the workspace

a Angles and and b velo cities and for the workspace

The inertial xed frame and the moving frame attached to the rigid b o dy

Motion of an ob ject following a straight line in a canonical co ordinates

of the rst kind b canonical co ordinates of the second typ e ordering

L L and c canonical co ordinates of the second typ e ordering

L L

Motions in a plane a a screw motion b a geo desic for metrics and

and c a geo desic after the b o dyxed frame fMg is displaced ix

Tra jectory of an ob ject following a a screw motion b a geo desic for the

metric and c a geo desic for the metric

Geo desics for dierent metrics on SE a pro duct metric

b a metric with and c a metric with

Tra jectories for zero b oundary conditions a the shortest distance path

b the minimumacceleration motion and c the minimumjerk motion

Minimumacceleration motions in plane for general b oundary conditions

T T T

a V V f g b V f g V f g

T T

g and c V f g V f

T

Minimumacceleration motions in space a V V f g

T T

b V f g V f g and c

T T

V f g V f g

Kinematic vs dynamic motion planning a minimumjerk b mini

mum torquechange preload forces not sp ecied and c minimum torque

change initial preload force N x

Chapter

Intro duction

In a basic rob otic task the rob ot has to movefromagiven initial conguration to a desired

nal conguration Except for some degenerate cases there will b e innitely many motions

that achieve the desired transition More complex tasks imp ose additional requirements on

the motion but in general the set of all p ossible motions will still b e very large A rob otic

task typically also entails manipulation of an ob ject and interactions with the environment

Toprevent damage to the manipulated ob ject or to the rob ot the manipulation and

interaction forces havetobecontrolled The purp ose of motion planning is to select

one motion from the set of all p ossible motions and to assure that the manipulation and

interaction forces remain within given b ounds

The relation b etween a kinematic tra jectory of a physical system and the actuator

forces that generate the motion along this tra jectory is given by dynamic equations In

general a motion plan thus consists of the kinematic tra jectory for the system as well as

the actuator forces that move the system along the tra jectory The actuator forces can

b e sometimes obtained from the given kinematic tra jectory in a particularly simple way

In other instances we use the kinematic description of the system b ecause the dynamic

equations are dicult to derive There are also cases when we employ the kinematic mo del

of the system to abstract the details of the actuation scheme In all these situations we only

need to plan the kinematic tra jectory for the system We will thus use the term dynamic

motion planning when the actuator forces are part of the computed motion plan and

kinematic motion planning when only the kinematic tra jectory for the system is computed

Wesay that motion planning is explicit if the motion plan is computed b efore the mo

tion is executed and implicit if the tra jectory and the actuator forces are computed while

the system moves Explicit schemes must b e used if wewant to optimize the p erformance

of the motion or guarantee certain prop erties of the tra jectory If it only matters that a

desired conguration is reached implicit schemes are sucient

Motion of a physical system can b e observed in three dierent spaces The task is

sp ecied in the task space The representation for this space is usually chosen to simplify

the eort of the human in describing the task In most applications Cartesian co ordinates

are used to sp ecify the p osition in space and some form of Euler angles is used to sp ecify

the orientation Task space description is convenient for humans but to prop erly represent

Throughout the dissertation the term actuator force will b e used to denote the generalized force

consisting of forces and torques pro duced by the actuators

a motion of a system with multiple degrees of freedom it is more appropriate to sp ecify

the motion in the joint space in dynamics the term used is conguration space The joint

space is the Cartesian pro duct of the intervals describing the allowable range of motion for

each degree of freedom Toachieve the motion some degrees of freedom must b e actuated

Wemust therefore compute the actuator forces that achieve the desired motion and these

forces b elong to the actuator space An actuator is usually controlled by a single input

currentorvoltage in the case of electromechanical actuators so the actuator space will

m

be typically a subset of IR where m is the numb er of actuators

Joint space Inverse Direct dynamics kinematics Dynamics

Inverse kinematics Task space Actuator space

Figure Spaces in which motion can b e observed and mappings b etween them

Mappings b etween the task space the joint space and the actuator space Figure

may not b e invertible For example in the case of a kinematically redundant rob ot there

is a mapping from the joint space to the task space but not viceversa a tra jectory in

the task space do es not sp ecify a unique tra jectory in the joint space Similarly if the

manipulator has actuator redundancy the mapping from the actuator space to the joint

space exists but it do es not in the other direction In general a curve in the actuator

space can always b e mapp ed to a curve in the joint space and a curve in the joint space

can always b e mapp ed to a curve in the task space The mapping from the actuator space

to the joint space is given by the equations of motion of the system and thus involves

integration of a set of dierential equations In contrast the mapping from the joint space

to the task space is algebraic although it might b e still dicult to obtain if the mechanism

involves parallel mo dules This implies that a tra jectory in the actuator space completely

characterizes the motion However for applications we usually also need the task space

and the joint space tra jectories and we require that they are part of the motion plan It is

also clear that with the exception of sp ecial cases in which the mappings b etween dierent

spaces are invertible the planner must provide the tra jectory in the actuator space to

completely sp ecify the motion

Motion planning has received muchattention in the past A short review of the liter

ature is provided in the next two sections Since humans seem to moveinavery ecient

way and since we often try to imitate humans by rob ots we start with the mo dels of

motion planning in h umans We then discuss algorithms that were develop ed for solving

the discretized motion planning problem Discretization reduces the innitedimensi onal

space of p ossible solutions to a nitedimensional search space One of the advantages of

discrete algorithms is that they can address the question of completeness if a solution in

the reduced space exists the algorithm will nd one if there is no solution the algorithm

will return the negative answer For continuous metho ds that we describ e next the ques

tion of completeness is much more dicult and in general completeness can not b e proved

Manycontinuous motion planning metho ds are based on optimal control However these

metho ds were used to address very sp ecic motion planning problems At the end of the

chapter we argue that optimal control and variational metho ds represent a natural frame

work for motion planning and we prop ose them as a general metho d We then formally

dene the dynamic and the kinematic motion planning problems that are addressed in the

dissertation

Motion planning in humans

Fascinating photographs by Muybridge originally published at the end of last century

are one of the earliest attempts to systematically study human and animal mo

tion In the s developments in control theory sparked interest in mo deling the organiz

ing principles of human motor control The mo dels that were develop ed in the subsequent

years can b e divided into two ma jor groups Mo dels in the rst group assume

that the motion is generated through a feedbackcontrol law Since the tra jectory

for the motion is not computed in advance these mo dels are examples of implicit planning

In contrast mo dels in the second group prop ose explicit schemes for motion planning and

assume that the tra jectory is planned in advance and then executed Exp er

imental evidence exists for the feedbackmechanisms as well as for the explicit tra jectory

planning and none of the existing mo dels seems to b e able to account for all the character

istics of human motion Nonetheless there is enough evidence to b elieve that mechanisms

for explicit motion planning do exist in the central nervous system

The human b o dy can b e mo deled as an articulated linkage of rigid b o dies This is also

ho wwe usually mo del rob ots Principles that govern human motion planning therefore

directly b ear on motion planning for rob otic systems Redundancies that make the map

pings b etween task joint and actuator spaces noninvertible are abundant in the human

body as well as other biological systems suggesting that humans p osses mechanisms for

resolving these redundancies Studies of human motion therefore represent an imp ortant

source of ideas for rob ot motion planning schemes Of particular relevance to rob otic

applications are studies of human arm motions

An imp ortant quantitativepropertyofhuman target directed motions was discovered

by Fitts Known as Fitts Law it relates the movement time to the traveled distance

and the size of the target Invariant prop erties of human arm tra jectories were studied by

So echting and Lacquanity and Viviani et al In it was observed that

the tra jectory for spatial motions is indep endentofmovementvelo city and that ratios

of velo cities of elb ow and shoulder joints are xed in the acceleratory and decceleratory

phases Twothirds p ower lawwas formulated in to describ e the relation b etween the

radius of curvature of the tra jectory and the tangential velo city Morasso showed that

ariant in the extracorp oral the shap e of the velo city proles for human arm tra jectories is inv

task space but not in the joint space and concluded that planning takes place in the

extracorp oral space

Feldman fo cussed on motions involving only the elb owjoint He treated the muscles

as nonlinear springs and showed that humans are able to mo dulate a parameter that cor

resp onds to the zerolength equilibrium p oint of the spring Based on these observations

he p ostulated the so called equilibrium p ointhyp othesis for the generation of voluntary

arm motions According to this hyp othesis to reach a goal conguration the equilibrium

p oint of the system of springs representing muscles of the human arm is instantaneously

shifted to the goal conguration The arm subsequently moves b ecause of the stiness of

the system

The hyp othesis that the system only sp ecies the target equilibrium p ointwas refuted

by Bizzi et al They showed that if the arm of a deaerented monkey is displaced

from the initial p osition it moves towards the initial p osition when the target at the nal

p osition is illuminated and only then changes the direction of motion and moves towards

the target This shows that a series of intermediate p ositions is planned and implies that

the nalp osition control according to which the motion is driven by the error b etween

the current p osition and the goal p osition and the equilibriu m p ointhyp otheses are not

sucient to describ e goal directed motions This study provides the strongest evidence for

explicit tra jectory planning

The results of Bizzi et al and the apparent ease and eciency of human movements

prompted Flash and Hogan to suggest that tra jectories for human planar reaching

motions are chosen so that they minimize the integral of the square norm of jerk jerk is

the derivative of acceleration

Z

t

y

x

dt J

t

In the equation t and t are the xed initial and nal times while x and y are the

Cartesian co ordinates of the hand in the plane of motion The minimumjerk modelas

it was called by the authors agreed with the measured human arm tra jectories during

medium sp eed large amplitude unconstrained motions in a horizontal plane Hogan

used the minimumjerk mo del to p ostulate the equilibrium tra jectory hyp othesis for the

formation of human motions In contrast to Feldmans equilibrium p ointhyp othesis Hogan

suggests that the equilibrium p oint of the system of springs representing the muscles is

gradually shifted along a prescrib ed tra jectory and not simply switched b etween the initial

and the nal conguration The tra jectory along which the equilibrium p ointmoves is a

minimumjerk tra jectory

In parallel with the work of Flash and Hogan Nelson also sp eculated that motions

are planned so that they minimize some global cost functional He discussed howcost

functionals such as time energyintegral of impulse and integral of jerk can b e used to

mo del tra jectories for violin b owing and jawmovements

Following these ideas Uno et al suggested another cost functional to mo del gen

eration of human single arm reaching motions They suggested that the motions minimize

the integral of the squared norm of the vector of torque derivatives

Z

n

t

X

J dt

i

t

i

As b efore t and t are the xed initial and nal time while is the vector of the joint

torques The authors presented exp eriments in which the minimum torquechange model

as it is usually called tted the data b etter than the minimumjerk mo del The mo del

also predicted kinematic features of the unrestrained vertical arm motions for which the

minimumjerk hyp othesis failed In addition they showed that the minimum torque

change tra jectories can b e computed with a layered neural network and argued that such

mechanism could b e implemented in the central nervous system

The minimumjerk and minimum torquechange mo dels agree quite well with

the exp erimental data it is still an op en question which paradigm b est describ es the pro

cesses that take place in the central nervous system during motion planning Although b oth

mo dels are based on the idea of minimizing an integral cost functional the implications

of the mo dels for the generation of human motion are quite dierent The minimumjerk

mo del suggests that only the task space tra jectory is planned This tra jectory is indep en

dentofthephysical structure dynamics of the system that p erforms the motion The

equilibrium tra jectory paradigm complements the minimumjerk mo del and explains how

the motion along the planned tra jectory is generated One of the main claims of this

mo del is that the tra jectory is planned separately at the kinematic task level and it is

subsequently transformed into joint tra jectories and actuator activations according to the

equilibrium tra jectory hyp othesis see also Unfortunately the equilibriu m tra jec

tory hyp othesis is very dicult to verify since the dynamic parameters of the human arm

damping stiness during motion are dicult to measure reliably

In contrast the premise of the minimum torquechange mo del is that an explicit plan

is computed in the joint space and the actuator space and that the planning pro cess is

p erformed in a single step thereby making the jointlevel and actuator level planning dep en

dent This is b ecause the cost functional dep ends on the actuator forces and incorp orates

the dynamics of the human arm into the planning pro cess The minimum torquechange

mo del provides an explicit tra jectory for the actuator forces and is easier to verify with

umjerk mo del exp eriments However it is computationally more intensive than the minim

since the dynamics of the system enters into the planning pro cess and has to b e mo deled

Tra jectory generation in rob otics

The division into explicit and implicit schemes for motion planning also applies to rob otics

In most cases explicit schemes are used for kinematic motion planning while implicit

schemes are usually employed for dynamic motion planning

An imp ortant concept that evolved through the study of kinematic motion planning

in rob otics is the notion of conguration space Although wellknown in mechanics it

was rst dened in the rob otics literature by LozanoPerez and Wesley The basic idea

is to represent the rob ot byapoint in an appropriate space Each element of this space

corresp onds to a dierent conguration of the rob ot from which the name conguration

in whichthe space In this space an obstacle Obs maps to a set of congurations C

i Obs

i

rob ot touches or p enetrates the obstacle Finding a collisionfree path for a rob ot thus

corresp onds to nding a tra jectory in the conguration space that do es not intersect any

of the sets C Although the computation of sets C is very time consuming it is

Obs Obs

i i

usually only p erformed once and a path b etween any arbitrary two congurations can b e

computed very eciently afterwards

Implicit schemes

Implicit schemes only use the information ab out the current state of the rob ot and the

environment to compute howtomove and can b e interpreted as feedback mechanisms

They are very attractive from a computational p oint of view since no pro cessing is required

prior to the motion The simplest scheme corresp onding to the nal p osition control in

biological systems is to make the setp oint for the jointcontrollers equal to the desired

nal p osition in the joint space and let the error b etween the current p osition and the

setp oint drive the rob ot A mo dication of this scheme where the velo city during the

motion is appropriately shap ed is often provided on industrial rob ots as one of the p ossible

mo des of motion but it is hardly useful for large amplitude motions One of the reasons

is that the shap e of the tra jectory in the task space dep ends on the lo cation of the start

and the end conguration within the joint space If obstacles are present in the workspace

of a rob ot it is dicult to predict whether the rob ot will avoid them or not

An approach analogous to Feldmans equilibrium p ointhyp othesis was develop ed

by Khatib He denes a p otential function with the equilibrium p oint at the goal

conguration The actuators of the rob ot are programmed to generate the force dictated

by the p otential eld driving the rob ot towards the goal conguration This scheme is

much more exible than the nal p osition control since the p otential eld that guides

the motion can b e chosen It is also easy to implement obstacle avoidance by assigning a

repulsive p otential to each obstacle this is how the metho d was rst used By making the

range of repulsive p otential limited only the obstacles that are close to the rob ot will aect

the motion The rob ot thus only needs to know lo cal information ab out the environment

If the p otential is dened in the joint space the problem of kinematic redundancy can

b e resolved as w ell The main drawback of the p otential eld approach is that

there may exist lo cal minima that can trap the rob ot Ko ditschek showed that if

obstacles are present there is no p otential function with a unique equilibrium p oint Rimon

and Ko ditschek demonstrated that a p otential function can b e constructed they call

it the navigation function which has a global minimum and for which all other equilibrium

p oints are saddlep oints unstable equilibria that lie in a set of measure zero However

constructing suchanavigation function requires complete knowledge of the space top ology

and manyadvantages of Khatibs approach are lost Another deciency of p otential elds

is that the generated tra jectories are usually far from b eing of minimallength Finally

it is dicult to takeinto accountvarious constraints p osed by the task suchasvelo city

limits or nonholonomic constraints

Explicit schemes

To compute a tra jectory explicit metho ds we could also call them op enlo op schemes

require knowledge of the global prop erties of the space The advantage of suchschemes

is that task requirements can b e taken into account during the planning pro cess The

approach is also attractive from the control p ointofview once the tra jectory of the

system is planned the system can b e linearized along this tra jectory and metho ds from

linear control theory can b e used to control its motion

An excellentoverview of metho ds for kinematic motion planning can b e found in

Latomb es b o ok Latomb e divides planning algorithms into three classes roadmap

cell decomp osition and p otential eld We discussed the p otential eld metho ds ab oveand

argued that they are basically implicit feedback metho ds Roadmap metho ds

construct a set of curves called roadmap that suciently connect the space

A path b etween two arbitrary p oints is found bycho osing a curve on the roadmap and con

necting each of the twopoints to this curve with a simple arc Instead cell decomp osition

metho ds divide the conguration space into nonoverlapping cells and con

struct a connectivity graph expressing the neighb orho o d relations b etween the cells The

cells are chosen so that a path b etween anytwopoints in the cell is easily found To nd

a tra jectory b etween two p oints in the conguration space a corridor is rst identied

by nding a path b etween the cells containing the two p oints in the connectivity graph

Subsequently a path in the conguration space is obtained by appropriately connecting

the cells that form the corridor

Kinematic motion planning metho ds describ ed in this and in the previous section have

b een mainly develop ed for mobile rob ots or socalled free ying agents Kinematic con

straints typical for a serial manipulator make the analysis more complicated although in

principle the metho ds can b e appropriately adapted An interesting example of a more

general metho d is the recursive algorithm for generating tra jectories for nonholonomic ve

hicles presented in The most general versions of roadmap and cell decomp osition

metho ds work for cases in which the obstacles in the conguration space can b e describ ed

as semialgebraic sets However most practical implementations assume that the ob

stacles and the rob ot can b e describ ed as p olygons At the price of considerably increased

complexity it is also p ossible to extend some of the approaches to cases in which the

obstacles in the environmentmove and their p osition is provided by sensors

A common feature of all the motion planning schemes describ ed in this section is that

they are based on discrete algorithms In one way or another the conguration space is

discretized and represented by a graph Subsequently tra jectory planning is reduced to

nding a path in this graph These metho ds are also purely kinematic they only generate

a tra jectory in the conguration space while the dynamics of the rob ot and the p ossible

constraints on the actuator forces are not taken into account To obtain a tra jectory in

the actuator space a separate mechanism must b e employed From the p oint of view of

hierarchical organization they therefore assume separate planning at each of the three

levels task space joint space actuator space

Continuous motion planning

We are interested in solving the motion planning problem without articially discretizing

the conguration space Such motion planning metho ds will b e called continuous A

go o d starting p oint is provided by studies of human arm motions Section Both the

minimumjerk mo del Equation and the minimum torquechange mo del Equation

are continuous mo dels for motion planning From the mathematical p oint of view b oth

schemes formulate motion planning as a variational problem However while the minimum

jerk cost functional only dep ends on the task variables the minimum torquechange cost

functional dep ends on the actuator torques In turn the dynamic equations represent

additional constraints and nding a solution for the minimum torquechange mo del requires

solving an optimal control problem We already noted that the minimum torque

change mo del is not a hierarchical planning metho d it yields a solution at jointand

actuator levels simultaneously

The problem of minimizing a functional on the tra jectories of a dynamical system

describ ed with a set of dierential equations is traditionally studied in optimal control

When the constraining equations are algebraic it is customary to use variational calculus

It therefore seems that optimal control corresp onds to dynamic motion planning while

variational calculus is a kinematic motion planning metho d We will show that such this

distinction is articial and that variational calculus and optimal control are two dierent

names for the same set of techniques For this reason we will use the term variational

metho ds to denote b oth optimal control and traditional variational calculus metho ds

The example of minimumjerk and minimum torquechange mo dels shows that vari

ational metho ds can b e used either for kinematic or for dynamic motion planning the

typ e of planning dep ends on the cost functional Variational metho ds have b een used in

rob otics in b oth roles long b efore similar ideas were used for mo deling human arm motions

After all the idea to move the rob ot along straight line paths in the task space whichis

probably as old as the rst rob ots see for a review of early work is just a sp ecial case

of variational approach where the cost functional is the Euclidean distance Most of the

work in rob otics that uses a variational approach is based on optimal control and addresses

the dynamic motion planning problem

Optimal control has b een extensively used in rob otics for timeoptimal control In

timeoptimal control the ob jective is to minimize the duration of motion b etween the

initial and goal congurations so that constraints on the actuator forces are satised The

problem was rst studied by Kahn and Roth The authors were able to show that for a

threelink serial mechanism with constant limits on the torques at least one of the actuators

op erates at the limit during timeoptimal motion They also prop osed an approximation

scheme based on linearization of the rob ot dynamics to compute the optimal solution

Timeoptimal control is also an alternative for planning tra jectories in the actuator

space when the path in the joint space is obtained with other metho ds see ab ove Bo

elop ed similar metho ds for brow et al and Shin and McKay indep endently dev

computing the timeoptimal control for a serial manipulator moving along a given path

They showed that this problem is equivalent to timeoptimal control of a double integrator

system and that the solution will thus b e bangbang the input is always equal to one of

the limits and it instantaneously switches b etween the limits nitely many times Based

on the denition of a velocity limit curve in the twodimensional state space they develop ed

an algorithm to compute the switching p oints Pfeier and Johanni and Slotine and

Yang further simplied this algorithm Huang and McClamro ch adapted the

metho d to contour following and found a solution for switching b etween free motion and

constrained motion Shiller and Lu generalized the algorithm from to handle

singular tra jectories Dahl extended the approaches from and to exible

manipulators

Theoretical results for timeoptimal control of mechanical linkages were derived in

Shiller and Dub owsky prop osed an algorithm for computing timeoptimal

inputs and tra jectories for a rob ot moving among obstacles They approximated the tra

jectories with Bsplines and used the algorithm from in conjunction with graphsearch

techniques to nd the timeoptimal tra jectory

Timeoptimal solutions require discontinuous jumps in the actuator forces But in

practice motors havetheirown dynamics and suchdiscontinuous jumps can not b e pro

duced Although minimum time solutions are attractive from a theoretical p ersp ective

timeoptimal control has limited practical applications For this reason some researchers

tried to combine timeoptimal control with approaches that yield smo oth actuator tra jec

tories One p ossibility is to minimize the combination of time and the integral of the square

norm of the vector of inputs The last integral is usually taken as a measure of the energy

consumed during the motion and the resulting problem is thus known as timeenergy opti

mal control Singh and Leu used dynamic programming to solve timeenergy optimal

control when the rob ot path is sp ecied Bessonnet and Lallemand computed time

energy optimal tra jectories and inputs using Pontryagins minimum principle A similar

problem was also solved by Shiller

In applications where time and limits on the actuator forces are not critical other cost

functionals can b e used to obtain smo oth motion plans Vukobratovic and Kircanski

prop osed minimization of energy approximated by the integral of the square norm of the

vector of inputs to compute optimal inputs for a degreeoffreedom anthrop omorphic

manipulator They assumed a known path and computed velo city distribution along the

path using dynamic programming Nakamura and Hanafusa prop osed optimal con

trol to resolve kinematic redundancy They studied several cost functionals square norm

of the velo city square norm of the force vector manipulability and used Pontryagins

minimum principle in conjunction with the sho oting metho d to nd the joint tra jectories

for a degreeoffreedom rob otic arm from the task space tra jectory Similar is the work

of Suh and Hollerbach who studied the minimization of energy as a metho d for re

solving kinematic redundancy They also compared global variational metho ds to lo cal

ed that the former are optimization schemes for resolving actuator redundancy and show

usually sup erior

An application of variational calculus to kinematic motion planning is the work of

Buckley He prop osed variational calculus as an alternative to discrete algorithms for

the tra jectory planning in the conguration space The main fo cus of his work was the

derivation of a distance function used to test the feasibility of the tra jectories Based on

this denition he derived an algorithm for tra jectory planning that favorably compared

with discrete tra jectory planning metho ds

Variational calculus was applied to motion planning for nonholonomic systems in Fer

nandes et al They studied motion of a falling cat where the nonintegrable non

holonomic constraint is the preservation of the angular momentum The motion planning

problem was p osed as the minimization of the L norm of the inputs and the Ritz metho d

was used to compute the solution

Avariational metho d is also prop osed in for op enlo op tra jectory planning for

nonlinear systems with no drift The b oundaryvalue problem that has to b e solved to

nd a feasible tra jectory is converted into a variational problem where the error at the

b oundaries is minimized This technique has b een extended in for planning tra jectories

for nonholonomic systems moving amidst obstacles

A rich b o dy of literature exists on hierarchical approaches to motion planning In

this work the emphasis is on the resolution of kinematic or actuator redundancy along

agiven task space tra jectory The general idea is to prescrib e a scalar cost functional

and nd its minimum at eachpoint along the task space tra jectory An example of such

approach is the use of a pseudoinverse in velo citycontrol of redundant manipulators

A similar approach for resolving actuator redundancy is prop osed in The ma jor

dierence b etween these metho ds and the variational approaches is that minimization in

the former case is p erformed lo cally and in nite dimensions while for the latter the search

is p erformed in functional innitedimensi onal space and globally optimal solutions can

b e obtained We therefore sp eak ab out local and global metho ds The advantage of lo cal

metho ds is that the solution can b e computed online during the motion while for global

metho ds the motion plan must b e computed in advance But since lo cal metho ds assume a

preceding stage in which the task space tra jectory is planned oline their advantage over

global metho ds seems questionable A discussion of some further shortcomings of lo cal

optimization schemes can b e found in and

At the end of this review wemention a class of continuous motion planning metho ds

of a rather dierentcharacter that are an alternativetovariational metho ds These tech

niques usually known as steering grew from research in nonlinear control and can

also b e viewed as constructive pro ofs of controllability They have b een very successfully

applied to nonholonomic systems Murray and Sastry showed that a large class of

nonholonomic systems can b e steered to a desired conguration using sinusoids A more

general theory was develop ed in and for driftfree systems An extension of the

work of Murray and Sastry can b e found in An imp ortantlinkbetween tra jectory

planning and control for nonholonomic systems is provided byWalsh et al who

ed with a nonholonomic rob ot Steer showed how a planned tra jectory can b e stably track

ing techniques were mainly develop ed for kinematic motion planning Recentadvances in

mo deling of systems with symmetry and mo deling of nonholonomic mechanical systems

make some of these ideas applicable to dynamic motion planning

An overview of the dierent approaches to motion planning that app ear in the literature

is shown in Figure Sp ecic metho ds are in the leaves of the tree the metho ds that

were develop ed for rob ot motion planning are underlined while the rest of the metho ds can

b e found in studies of human motion The frame around the word variational denotes

that the fo cus in the dissertation will b e on variational metho ds

Approach in the dissertation

In most of the literature motion planning is dened as nding a suitable path b etween

two p oints in space In this work wetakeamuch broader view so that the motion plan

includes every asp ect of the motion that is necessary to p erform the task This broader

denition in a waycombines what is traditionally understo o d as motion planning with task

planning that is typically p erformed at a more abstract level For example if the task is

to grasp an ob ject with a multingered hand weareinterested in howtoformagrasp

move the ob ject control the force so that the ob ject is rmly held but not crushed and

to regrasp the ob ject when necessary Similarly when planning motion for a multilegged

rob ot we are concerned with the gait pattern placement of the legs on the ground and

timing of events during walking

The metho ds develop ed in this dissertation fall into the class of variational or optimal

control approaches As indicated throughout the chapter variational metho ds have several

advantages

They can b e used as dynamic motion planning metho ds They can therefore take Motion Planning

Implicit

Explicit Potential Fields Final Position Control Equilibrium point

Dynamic Motion Continuous Planning Discrete

Steering Variational Cell decomposition Roadmap

Minimum Energy, Time Minimum Length Kinematic

Min. Torque−Change Minimum Jerk Motion Planning

Figure Division of the approaches to motion planning found in the literature

into account the dynamics of the system and provide the tra jectories at all three

levels task space joint space and actuator space Most of the other metho ds only

provide kinematic tra jectories

By minimizing a cost functional we nd a tra jectory and resolve kinematic and

actuator redundancies within the same framework In other approaches dierent

strategies are employed at dierentlevels

Variational metho ds provide a unifying framework for dealing with equalityand

inequality constraints In particular since the dynamics of the system can b e taken

into account nonholonomic constraints b ecome simply equality constraints on the

state space and can easily b e treated

Variational metho ds can yield a feasible and globally optimal solution In contrast

kinematic motion planning is not concerned with the limitations on the actuator

capabilities and might pro duce solutions that are not suitable for implementation

This is also a disadvantage of lo cal optimization schemes

By cho osing variational metho ds for motion planning wemake some imp ortant assump

tions The critical assumption is that wehave a go o d mo del of the system In this work

we also assume that the mo del is completely deterministic These assumptions limit the

generality of the prop osed approach but they are satised in many practical applications

By cho osing an explicit scheme for motion planning we also assume that we can synthesize

a controller that can follow the computed tra jectories Suchcontrollers can b e readily

synthesized if the system is controllable

We will showthatvariational calculus and optimal control are equivalent and can b e

used for kinematic or dynamic motion planning dep ending on the chosen cost functional

In the rst part of this dissertation we will develop a general metho d for planning tra

jectories and actuator forces Due to physical limitations of a rob ot and b ecause of the

nature of interactions of a rob ot with the environment a rob otic task typically involves

equality and inequality constraints The problem that we will address in the rst part can

b e therefore describ ed as follows

Problem Dynamic motion planning Let the robot be described in the state space

with the equations

x f x u t

where x is the vector of state variables and u is the vector of inputs Suppose that during

the motion the robot must satisfy a set of equality and inequality constraints

g x u t i k

i

and

h x u t i l

i

and let the initial and nal congurations begivenby

x tj

t

and

x tj

t

Choose a cost functional

Z

t

J xt t L xtutt dt

t

The functions f g h and L areassumedtobe of class C in al l variables while the

and are assumedtobe of class C

functions The motion planning problem is to find

t u t (a piecewise smooth) state vector x and (a piecewise continuous, bounded) input vector that

satisfy equations (1.3)-(1.7) and minimize the cost functional (1.8).

Weareinterested in motion planning for mechanical systems Continuity requirements

on the functions f g and h are therefore not problematic since the dynamic equations for

mechanical systems are suciently well b ehaved Further wegettocho ose the function

L so it can b e made suciently smo oth However if the task involves impacts the

impact forces will b e impulsive and the state will b e discontinuous In this work wedo

not consider such cases but the metho d for motion planning for systems with changing

dynamic b ehavior that we describ e in Chapter provides some insightinhow such problems

can b e addressed

Kinematic motion planning is simpler and more ecient than dynamic motion planning

It is thus preferred to dynamic motion planning if there exists a suitable way to compute

the actuator forces from the kinematic tra jectory Kinematic motion planning is also the

only feasible alternative when a dynamic mo del of the system is to o complicated or is not

available and is abstracted with a kinematic mo del

Kinematic motion planning can b e p erformed either in the task space or in the joint

space The task space has often more structure and the planning metho d must b e carefully

develop ed to take advantage of this structure For this reason we concentrate in the second

part of the dissertation on variational metho ds for kinematic motion planning in the task

space The develop ed approach can b e directly applied to the kinematic planning in the

joint space To develop a general metho d we will p ose kinematic motion planning as a

variational problem on a Riemannian manifold

Problem Tra jectory planning Let the task spacebe described with a manifold

and take a Riemannian metric on Choose the desired initial and nal

congurations p and p for the robot in the task spacep p and let C and C

be the additional conditions that have to be satised at these two points Take the family

G of al l smooth curves that start at p end at p and satisfy C and C

G f j p p C C g Dene a function L GT and a

functional J GIR of the form

Z

d d

L dt J L

dt dt

G

The trajectory planning problem on is to find a curve that minimizes the functional (1.9).

Contents of the dissertation

The dissertation will b e roughly divided into two parts Chapters to will address nding

a solution to Problem and present some applications of dynamic motion planning

In Chapter we will briey review the basic theory of optimal control and state the

Pontryagins minimum principle We will show that a general optimal control problem

with constraints can b e formulated as a problem of Bolza in the calculus of variations if

the constraints can b e describ ed with a set of equality and inequality constraints We

will formulate the necessary conditions for the solution of the variational problem and

compare these conditions with those provided by the minimum principle for the optimal

control problem to establish the exact mapping b etween the two problems We will then

show that the problem of Bolza can b e transformed into an unconstrained problem in the

calculus of variations that has the same set of extremals

We will briey review numerical metho ds for solving optimal control problems in Chap

ter We will discuss a rstorder metho d for solving Problem that exploits the equiva

lence of extremals of the problem of Bolza and the corresp onding unconstrained variational

problem A detailed analysis of the metho d for problems with equality and inequality con

straints that only dep end on the state variables will b e presented Since this metho d can fail

to converge to a minimum wewilldevelop a direct minimization metho d by approximat

ing the continuous problem with a sequence of discrete nonlinear programming problems

Metho ds for solving the resulting nonlinear programming problems will also b e discussed

At the end of the chapter we will address how an appropriate cost functional for motion

planning can b e chosen We will explain why some cost functionals are only suitable for

kinematic motion planning and discuss functionals suitable for planning smo oth motions

In Chapter we will demonstrate the metho ds from Chapter on the problem of

planning tra jectories and actuator forces for two planar arms co op eratively manipulating

an ob ject We will start with a simple case when no constraints on the contact forces

are present A more complete treatment will follow in which frictional constraints will b e

taken into account

Chapter will provide additional motivation for using variational metho ds for motion

planning Wewillshow that the metho d prop osed for planning the motion for rob otic

systems can b e applied to mo deling human arm motions Exp erimental investigation of

human twoarm manipulation will b e presented and a mo del based on the results from

Chapter prop osed for mo deling the measured data This chapter also shows that the

interaction b etween motion planning for biological and rob otic systems go es in b oth di

rections principles discovered for biological systems can b e used for motion planning in

rob otics while metho ds devised for generating motion plans in rob otics are p ossible mo dels

for biological systems

Chapter represents a link b etween our framework for motion planning and algorithmic

approaches to rob ot task planning The metho d from Chapter will b e extended to

generate motion plans for a rob otic system whose dynamic equations change as it evolves

in the state space A framework for describing such systems in the state space will b e

develop ed and we will p ose the optimal control problem We discuss the complexity of this

problem and present a partial solution that can b e applied in several rob otic applications

The metho d will b e demonstrated on an example of a twongered hand manipulating a

circular ob ject

Chapters and form the second part of the dissertation and will establish a general

framework for kinematic motion planning in the task space Some background material

from Riemannian geometry will b e briey reviewed in Chapter Since in most cases the

task space manifold is the Euclidean group of rigid b o dy motions SE we will devote the

rest of the chapter to the study of the geometric structure of this group It will b e shown

that dierent applications in rob otics demand dierent Riemannian metrics on SE In

particular we will lo ok at the metrics suitable for the study of screw motions and discuss

connections that are appropriate for kinematic analysis

The analysis from Chapter will b e used in Chapter to study tra jectory planning on

SE We will discuss the invariance of tra jectories with resp ect to the choice of co ordinate

frames and intro duce a framework for variational calculus on Riemannian manifolds We

will showhow cost functionals such as minimumjerk that are used in studies of human arm

motions can b e extended to an arbitrary manifold We will also show that in some sp ecial

cases the expressions for the tra jectories minimizing these cost functionals are esp ecially

simple At the end of the chapter we will show some numerically computed tra jectories

to illustrate their dep endence on the choice of the metric reference frames and b oundary

conditions

Chapter concludes the dissertation There we will summarize our results and p ointto

the contributions of this work We will discuss p ossible applications and identify directions

for future research

Chapter

Relation between optimal control

and calculus of variations

The central problem of optimal control is to drive an underdetermined dynamical sys

tem from an initial state to the desired nal state so that a chosen cost functional is

minimized This problem has b een already studied by Lagrange in the th century Im

portantcontributions towards the solution of the problem were made byMayer and Bolza

at the turn of the century and later by Bliss and his students However optimal control

is usually regarded as an outgrowth from the control theory as develop ed after the second

world war The two most imp ortant formal works that established the foundations for the

subsequent research in optimal control were Bellmans principle of optimality of dynamic

programming and the minimum principle develop ed byPontryagin and collab orators

Applications in aviation and aerospace have fostered rich activity in the area in the

following decade Although it so on b ecame apparent that almost all practical problems

are intractable analytically advances in computer technology and numerical metho ds have

made numerical solutions to many optimal control problems feasible and there has b een a

steady interest in the practical applications of optimal control

The formulation by Bolza is often more convenient for describing motion planning prob

lems than optimal control Howev er most results in the control literature are derived from

the minimum principle and it is therefore not clear how they relate to the more classical

results from the calculus of variations In this chapter we establish some of these relations

with the aim of using the Bolza formulation to develop a numerical metho d for solving the

motion planning problem We rst state the Pontryagins minimum principle for the

general case Weshow that the problem can b e reformulated as the problem of Bolza in

the calculus of variations if the set of admissible controls can b e describ ed by equalityand

inequality constraints We state the rstorder necessary conditions for a solution of the

variational problem and demonstrate that these necessary conditions are a subset of the

conditions given by the minimum principle In the pro cess we also nd relations b etween

variables in the optimal control formulation and the variational formulation Finallywe

showthatforavariational problem with equality and inequality constraints it is p ossible

to formulate an unconstrained variational problem that has the same extremals stationary

p oints

Optimal control problem and Pontryagins minimum

principle

In this section we dene the basic setting for study of optimal control We consider

a dynamical system describ ed by an underdetermined system of rst order dierential

equations

x f x u t

where the ndimensional vector x is the state of the system and the mdimensional vector

u represents the controls inputs of the system The system is underdetermined

since the controls u can b e arbitrarily chosen Once u is chosen the system b ecomes

n

determined The state space will b e assumed to b e the Euclidean space IR and the vector

m

of controls u to b e a function u IR U IR where the set U can b e arbitrary We

dene the set of admissible controls U to consist of those functions u that are b ounded

n n

R IR is C and piecewise continuous We also assume that the function f IR U I

for all its arguments Given a vector u the existence and uniqueness of the solution to

is therefore guaranteed for arbitrary initial state x

A transition of the dynamical system from one state to another is called a motion Let

the initial and the desired nal state for the dynamical system b e given by

xt x and xt x

and dene the cost of a motion to b e the value of the functional

Z

t

Lx u tdt J xt t

t

Here t and t are the initial and nal time resp ectively The function L is assumed to b e

of the same class as the function f The optimal control problem is

Problem Optimal control Consider the dynamical system described by Equation

J of the form

and a functional The optimal control problem is to find among all the

u U x

admissible controls the control which moves the system (2.1) from the initial state to the desired x

final state and minimizes the functional (2.3).

In the rest of this work we will assume that given a certain top ology on the set of piecewise

smo oth functions there exists an op en set of functions ut that can bring the system

from the initial state x to the nal state x If this were not the case it would not make

sense to talk ab out the minimization of the functional J Problems where this assumption

is satised are also called normal

Without further assumptions we can not say whether the optimal solution exists and

whether it is unique However Pontryagin et al showed that if the optimal solu

tion exists it must satisfy the necessary conditions known as the Pontryagins minimum

principle

See for a more general setting of the problem



The choice is usually b etween so called strong and weak top ologies

Theorem Pontryagins minimum principle Dene the Hamiltonian

T

H x u t Lx u t f x u t

If a control u t is optimal and it generates a trajectory x tthenthere exists a nonzero

t of the vector

solution adjoint equation

H

T

x

such that for every t t t and for every admissible u U

H x u t H x u t

Pontryagins minimum principle therefore identies the set of inputs that could minimize

the cost functional The vector t has dimension n and its comp onents are called

the adjoint variables Note also that the system equations can b e rewritten as

H

x

The minimum principle is very general and places no restrictions on the set U

However Equation is dicult to convert to an ecientnumerical pro cedure A more

convenient expression can b e obtained if the set U is op en In this case Equation

implies that the optimal input u satises

H x u t

u

Remark The optimality condition is just the rstorder necessary condition for

and is therefore weaker

Minima of the cost functional b elong to a broader class of curves that render this

cost functional stationary Such curves are called extremals and play the central role in

the theory of optimal control and the calculus of variations When the set U is op en the

stationary p oints of the functional are exactly the curves that satisfy Equation

In this case the extremals are therefore given by the following set of dierential equations

H x u t

x

T

H x u t

x

where the input u is calculated from If the initial state x and the nal state x

are given they provide all the necessary b oundary conditions for the system If the

k

value x is not sp ecied the missing condition must b e replaced by see

k

k

Similarly if a comp onent x is missing the b oundary condition b ecomes

k

k

x

t

Equations are also called the natural boundary conditions and are a sp ecial

case of so called transversality conditions

In general it is hard to obtain results ab out the existence and uniqueness of the so

lutions of the b oundary value problem and this is the main reason that such results

are also dicult to state for optimal control problems Another question is how realistic

is the assumption that the set U is op en We will show that when the b oundaries of the

set U can b e expressed analytically the machinery of Lagrange multipliers can b e used

to convert the problem with constrained set U into one where U is the whole space and

therefore op en Since this is the case in most applications the minimum principle has

found widespread use

Supp ose therefore that the set of admissible controls consists of all b ounded piecewise

m

continuous functions u IR IR that satisfy a set of equality and inequality constraints

g x u t i k

i

and

h x u t i l

i

For the problem to b e meaningful we require that k m The minimum principle in this

case can b e restated in the unconstrained form

Prop osition Unconstrained formulation Dene the Hamiltonian

T T T

f g h H L

wherethevectors and have dimensions k and l respectively If the optimal control

problem has a solution then there exists a nonzero solution t of the adjoint equation

H

T

x

and the optimal input u t satises the equation

H x u t f g h L

T T T

u u u u u

The optimal solution x t and the optimal input u t must satisfy Equations and and in addition the fol lowing

complementarity conditions must hold

if h

i

i

if h

i

We note that when the set of admissible controls is given by the Equations

the optimal control problem is exactly the same as the motion planning problem

Variational reformulation of the optimal control prob

lem

An alternative approachtosolve Problem is to observe that it can b e converted into a

problem in the calculus of variations known as the problem of Bolza The problem of

Bolza was dened at the b eginning of the century by Bolza and is the closest in formulation

to the optimal control problem However it can b e shown to b e equivalent to problems of

Lagrange and Mayer whichwere dened much earlier These problems were extensively

studied by Bliss and his students at the University of Chicago in the s and s and the

results are describ ed in Formally the problem of Bolza is dened as

r X t Problem Bolza Find a piecewise smooth ( -dimensional vector) function that mini-

mizes the functional:

Z

t

J X t t LX u tdt

t

subject to the side constraints:

X X t i p p r i

the initial condition

X t X

and the final condition

X t X

We again assume that the problem is normal that is that there exists an op en set in

an appropriate top ology of functions X t that solve the b oundary value problem

If this were not the case this b oundary value problem would only have isolated

solutions and it would not make sense to talk ab out a variation of the functional J

To establish the relation b etween the optimal control problem and the problem of Bolza

we rst prove the following a Lemm If the set of admissible controls for the optimal control problem 2.1 can be described with a set of equality and inequality constraints, the optimal control problem can be formulated as a problem

of Bolza 2.5.

version of Problem into a problem of Bolza involves steps

Proof: Con

Intro duce a vector of slack variables of dimension l and dene

h x u t i l

i i i

where the functions h are those from It is easy to see that the inequality

i

constraints are equivalent to the equality constraints

x u t i l

i

This conversion of inequality constraints into equality constraints was prop osed by

Valentine The slackvariables can b e interpreted as additional inputs to the

system

Let r n m l and let

x X i n

i i

u X i m

i ni

X i l

i nmi

Note that the inputs and the slackvariables are integrated when converted to the

variables of the problem of Bolza The unknowns for the variational problem are

therefore smo other

Dene

f X i n

i i i

g i k

ni i

i l

nk i i

where the functions f and g are those from Equations and while the

i i

functions were dened in F or the problem to b e meaningful we assume

i

k m

The last step consists of renaming the parameters of the functions and L in the

functional to reect denitions

X X t x t

LX X t Lx u t

The ab ove reduction was rst prop osed by Hestenes and was later used in and

to study optimal control problems through the theory of the calculus of variations

Remark The reduction of the optimal control problem to a well understo o d problem

in the calculus of variations that was known b efore the minimum principle was published

suggests that optimal control could b e studied within the classical theory of the

calculus of variations When Pontryagin and his coworkers derived the minimum principle

the classical theory was derived for piecewise smooth functions dened over some open

set in the appropriate space Instead the minimum principle was derived for admissible

controls b eing measurable and for arbitrary choice of the range space U of controls

At the time the theory of optimal control was therefore more advanced than the classical

theory of the calculus of variations at least as far as necessary conditions are concerned

The necessary conditions for the solution of Problem can b e obtained using classical

results from the calculus of variations A study of the rst variation of the functional

yields the following rstorder necessary condition

Prop osition Dene an admissible variation Z ttobeapiecewise smooth function

such that Z t Z t Ifapiecewise smooth function X t is a solution of Problem

then there exists a nonzerovector of Lagrange multipliers and a

nk l

function H denedby

nk l

X

H L

i i

i

such that

T T

Z

t

H H

dt Z Z

X

X t

for any admissible variation Z In addition the side constraints

must hold

With some further manipulation we can eliminate the need for explicitly considering ad

missible variations in the ab ove theorem The necessary conditions for the solution of the

problem of Bolza thus b ecome

Theorem EulerLagrange equations If a piecewise smooth function X t is a

solution of Problem then there exists a nonzerovector of Lagrange multipliers

and a function H dened by

nk l

nk l

X

H L

i i

i

oint wherethevector X t is smooth the fol lowing

such that at every p Euler-Lagrange

equations hold

H H d

dt X

X

tapoint t where X t has jump discontinuity the so cal led A Weierstrass-Erdmann corner

conditions must hold

H H

X X



t t

H H

X H H X

X X



t t

In addition the n k l side constraints

must hold everywhere

Since the rst order necessary conditions are exactly the conditions that the functional

is made stationary the functions that satisfy the EulerLagrange equations are

precisely the extremals

Equivalence of the extremals

In this section we nd the relation b etween the extremals of the optimal control problem

and the extremals of the corresp onding variational problem The main result is

Prop osition Suppose that the set U in the statement of Problem can be described

with a set of equalities and inequalities Then the fol lowing statements hold

a If a piecewise smooth function X t satises the rst order necessary conditions for

the optimal control problem statedinProposition it also satises the rst order

necessary conditions of the equivalent problem of Bolza given by Theorem

b If X t satises the rst order necessary conditions for the Bolza problem given in

Theorem and if the Lagrange multipliers corresponding to the inequality con

straints are everywhere nonnegative then X t also satises the rst order necessary

conditions in Proposition for the optimal control problem

trol problem with equality

Proof: The rstorder necessary conditions for the optimal con

and inequality constraints were stated in Prop osition Following the prop osition we

dene

T T T

H L f g h

The equations for the extremals are therefore the system equations

x f

the adjoint equations

L f g h

T T T T

x x x x

the equality constraints

g

the optimality conditions

L f g h

T T T

u u u u

and the complementarity conditions

if h

i

i

if h

i

Note that the complementarity conditions can b e rewritten as

T

h

Wenow derive the equations for the extremals of the equivalent problem of Bolza

T T T T

Following the four steps from the pro of of Lemma we dene X X X X where

X x i n

i

i

X u i m

i

i

X i l

i

i

T T T T

We partition the vector of Lagrange multipliers from Theorem into

where the dimension of is n the dimension of is k and the dimension of is l

T T T T

Analogouslywe dene the vector of side constraints by

f X i n

i

i i

g i k

i

i

h i l

i

i i

Following Theorem we dene

T T T

H L

Note that the function H do es not dep end on X and X but only on their time derivatives

Since the side constraints must hold we rst obtain

f X

g

which are Equations and Further according to the denition the

equation

H d H

dt X

X

leads to

L d

T T T T

dt X X X X

It is not dicult to see that this is exactly Equation if we put and

Next take the equation

H d H

dt X

X

Since H do es not dep end on X the equation b ecomes

H d

dt

X

and after integration

H

const

X

There are no b oundary conditions sp ecied for X We are only interested in u X

which means that an arbitrary constant can b e added to X without changing the value

of usowe can set

X j

t

On the other b oundarywemust enforce the so called natural boundary condition

H

X

t

H

is continuous along the From Prop osition it can also b e seen that the function

X

extremal We therefore conclude

H

X

which b ecomes

L

T T T

X X X X

This equation is the same as the optimality condition Finallywe consider the

equation

H H d

dt X

X

Following similar reasoning as ab ove this equation yields

H

X

Using the denitions for H and the equation b ecomes

i l

i

i

This equation can b e rewritten as

i l

i

i

and nally by expressing from

h i l

i

i

Since the last equation will b e satised if the complementarity condition is

i

i

satised which proves a Further if this equation is exactly the complementarity

condition which also proves b

Remark We could prove a stronger claim that the minimizing extremals of b oth

problems are the same by using secondorder necessary conditions In this case the con

dition would follow from the Legendre condition H

X X

Corollary If the optimal control problem is reformulatedastheproblem of Bolza

the fol lowing relations hold between the Lagrange multipliers of the two problems

This relations will b e imp ortant when we discuss the case in which the functions h and g

do not dep end explicitly on the inputs u

The next observation is that the Lagrange multipliers in the variational formulation

can b e replaced with new functions where

With this substitution it is not dicult to see from Equation that

H

Further if a natural b oundary conditions are enforced on att the side constraintis

equivalent to the equation

H d

dt

Since the function H do es not dep end on the last equation has the form of the Euler

Lagrange equation for the variable This leads to

Prop osition The extremal X t and the set of Lagrange multipliers t for the

problem of minimizing the cost function

Z

t

LX X tdt J X t t

t

subject to the side constraints

X X t

T

T T

correspond to the extremal Y t X t t of the problem of minimizing the cost

function

Z

t

T

LX X t X X t dt J X t t

t

where

This prop osition shows that to nd the extremals not necessarily minimizing the varia

tional calculus problem with side constraints can b e converted into an equivalent problem

without side constraints if the Lagrange multipliers are treated in the same wayasthe

i

unknown functions In other words

Formulation

Formulation

T

T T

Critical p oint Y X of

Critical p oint X of

Z

Z

t

t

J X t t LX X dt

H Y Y dt J X t t

t

t

sub ject to

where

T

X X

H Y Y LX X X X

It is imp ortant to note that the rst order necessary conditions do not distinguish

between maxima minima and inection p oints This implies that it would b e wrong to

conclude on the basis of Prop osition that a critical p oint that is a minimum for

Formulation must also b e a minimum for Formulation Tocheck if an extremal is a

minimum wemust employ second order necessary conditions The second order necessary

condition also known as the LegendreClebsch necessary condition for X to minimize the

cost functional J in Formulation is that the matrix G where

X X

T

G L

is p ositive denite For Formulation the necessary condition that X minimizes

the cost functional J is that the matrix H is p ositive denite where Y is the vector

Y Y

X

Y

Now assume that the only constraints are the system equations The function is thus

given by

i n X X f X X

i i

In this case the LegendreClebsch necessary condition for Formulation b ecomes H

uu

We also have

x

Y Y

u

Y

where

u Y

u

Y

A short calculation shows that the matrix H has the following form

Y Y

I

nn nm nn



H

H

mn  mn

Y Y

u

I

nn nm nn

This matrix is indenite regardless of the value of the matrix H which dep ends on x u

uu

and This implies that for Formulation no critical p ointisaminimum for J Lagrange

multipliers can therefore not b e used to convert a constrained minimization problem to

an unconstrained minimization problem

Remark If the variational problem is convex the cost functional in the Formulation

must b e actually maximized over the Lagrange multipliers

Regular and singular extremals

Take the Bolza problem with r unknown functions the dimension of the vector X t

and s side constraints the dimension of the vector An extremal X t for the problem

of Bolza is called regularif

H H

s

X X

r

det for all t t t

X X

X  X t

r s

t

where is the vector of Lagrange multipliers corresp onding to the extremal X t An

extremal which is not regular is called singular An analogous denition exists for

the optimal control problems

If an extremal is regular it is p ossible to reduce the EulerLagrange equations and the

side constraints to a system of r rst order dierential equations see pp

In other words the eld of extremals all the solutions of the EulerLagrange dierential

equation with no b oundary conditions imp osed can b e emb edded into a r parameter

family of curves This is a go o d indication that we can satisfy r b oundary conditions

but it is not sucient On the other hand even if the extremal is singular we might

b e able to satisfy the b oundary conditions in some sp ecial cases A singular extremal can

b e compared to an overconstrained system of linear equations In general such a system

do es not admit a solution but if the rank of the system matrix is the same as the rank of

the extended system matrix a solution exists

Numerical metho ds often pro duce oscillatory solutions along singular extremals This

can b e explained by the fact that the rst nonzero term in the Taylor series expansion

of the cost functional along a singular arc is of the fourth order at a minimum the rst

nonzero term in the Taylor series must b e of the even order otherwise the v ariation would

change sign The inuence of the solution on the value of the cost functional is therefore

very weak and as a result the numerical metho ds are illconditione d The common

practice in such cases is to add a term to the cost functional that makes the problem

nonsingular and then gradually decrease this term to zero

Chapter

Numerical metho d

In this chapter we presenttwo metho ds for nding extremals of the motion planning

problem numericallyWe start with a short overview of numerical metho ds for optimal

control We next presentanumerical metho d for computing the extremals of the problem

of Bolza that was prop osed by Gregory and Lin The metho d nds a solution that

satises the rst order necessary conditions for the extremal but can not distinguish

between a lo cal minimum or a lo cal maximum We p oint to some shortcomings of the

metho d if it is applied to problems with equality and inequality constraints and prop ose

appropriate mo dications Next we describ e a novel metho d for direct minimization of a

cost functional The continuous problem is discretized by using ideas from niteelement

analysis techniques from nonlinear programming are used to solve the resulting nite

dimensional optimization problem We review the metho d of augmented Lagrangian for

solving constraint problems and Newtons metho d for minimization We show that the

expressions for the gradients that are used in minimization can b e easily computed if the

prop osed scheme is used to discretize the continuous problem Finallywe discuss the

choice of cost functionals that can b e used to formulate the motion planning problem and

demonstrate that the cost functional determines whether a motion planning or a tra jectory

planning problem is solved

Numerical metho ds for solving optimal control prob

lems

Anumber of numerical metho ds have b een develop ed over the years for solving optimal

control problems Most metho ds were develop ed for sp ecic applications and there is no

consensus on a go o d general metho d The main classes of metho ds are

Indirect metho ds The extremals are computed from the minimum principle rst

order necessary conditions bysolvingat wop oint b oundary value problem The

most p opular technique for solving b oundary value problems is to guess the missing

initial values and improve the guess until the b oundary values are matched If the

initial guess is close to the exact solution this metho d can b e shown to converge For

obvious reason the technique is also called the method of neighboring extremals

The main drawback of the metho d is that it is very sensitive to the initial guess

Direct metho ds The cost functional is minimized with resp ect to u directly by

J

In analogy with the minimization of functions in nite computing the gradient

u

dimensions rst order and second order gradient metho ds can b e formulated as well

as conjugate gradient algorithms In the earlier metho ds the gradientof

with resp ect to u was computed by a pro cedure known as backward sweep and

the metho ds were conceptually very similar to the metho d of neighb oring extremals

More recent algorithms approximate the gradient directions of the original

problem by approximating it with a simpler optimal control problem These metho ds

are robust and globally convergent They usually deal with constraints by adjoining

them to the cost functional in the form of penalty functions

Discretization metho ds The continuous problem is discretized and transformed into

a nitedimensional problem In this way the integral in b ecomes a nite sum

and metho ds of nitedimensional mathematical programming can b e used to nd a

solution Some metho ds only discretize the vector of controls while the others

discretize b oth the state and controls Another p ossibilityistointerp olate the

unknown functions with a set of basis functions collo cation metho ds As with

the direct metho ds the constraints are often adjoined to the cost functional with the

p enalty functions

Dynamic programming The idea is to discretize the state space and compute the op

timal value of the cost functional for each grid p ointtaken as the initial state for the

optimization This value is called the optimal return function and is computed by

a recursive algorithm Given the values of the optimal return function it is easy

to construct an extremal starting at an arbitrary initial p oint Dynamic program

ming always pro duces a global minimum but the computation b ecomes prohibitively

exp ensive as the dimension of the state space increases

e to the neighb oring extremals metho d is to solve the An indirect metho d alternativ

twopoint b oundary value problem resulting from the rstorder necessary conditions

with a nite dierence metho ds By its nature it is a combination of the indirect

metho ds and discretization metho ds the unknowns and the rstorder necessary

conditions are discretized so that a twop oint b oundary value problem is converted

into a system of algebraic equations

In deciding whichnumerical metho d to use for our work wewere guided bya number

of criteria Any realistic motion planning problem contains inequality constraints The

metho d should therefore b e able to easily handle such constraints preferably without any

extra computations Further the metho d must b e applicable to a wide variety of dynamical

systems that app ear in rob otic applications redundant and overactuated manipulators

multiple co op erating manipulators walking machines etc It is also desirable that equal

ity constraints apart from the dynamic equations can b e handled without elimination of

variables This is esp ecially imp ortant when the system contains closed kinematic lo ops

resulting in kinematic closure equations that can not b e solved analytically Similarlyit

is advantageous if the dynamic equations can b e implicit Implicit equations are typically

obtained after elimination of Lagrange multipliers which are used to derive dynamic equa

tions for systems with constraints Since most of these requirements can b e met if the

motion planning problem is formulated as the problem of Bolza we concentrate on the

metho ds that are appropriate for solving problems in this form

Firstorder metho d for nding the extremals

The aim of rstorder metho ds is to nd an extremal of the variational problem that is a

curve that satises the rstorder optimality conditions Since an extremal is not necessary

a minimum for the variational problem these metho ds are usually used when we can get a

go o d initial guess for a minimizing extremal or in combination with a direct metho d after

this has approached the minimum

Traditionally the extremals are obtained by solving the EulerLagrange equations

However the EulerLagrange equations only hold at p oints where the extremal

X t is smo oth At p oints where X t has jumps these p oints are called corners the

WeierstrassErdmann corner conditions must b e met The lo cation of the corners

their numb er and the amplitudes of the jumps in X are not known in advance so it is di

cult to obtain a numerical metho d for a general problem using EulerLagrange equations

An alternativeway of computing the extremals was suggested by Gregory and Lin

They based their numerical metho d on the necessary conditions in the integral form

Theorem whichsays that if X t is an extremal the rst variation of the functional

must vanish for every piecewise smo oth admissible variation Z t

T T

Z

t

H H

Z dt Z

X

X

t

Since the EulerLagrange equations and the WeierstrassErdmann corner conditions are

derived from Equation no information is lost by pro ceeding this way

In Prop osition we showed that for any problem with side constraints wecan

construct a problem with no side constraints that has the same extremals In deriving

numerical metho d we therefore assume that wehave to nd an extremal for the variational

problem

Z

t

J X t t LX u tdt

t

and there are no side constraints Let the numb er of the unknown functions dimension

of X be M To compute the solution numericallywe discretize the interval t t with

a sequence of p oints t a a a t For simplicitywe assume that

N

a a h for i N Next for k N weintro duce a set of piecewise

i i

linear shap e functions

ta

k

if a t a

k  k

h

a t

k 

t

if a t a

k

k k

h

otherwise

A graph of the shap e function t is shown in Figure

k

Since Equation must hold for any admissible variation Z t it must also hold if

th

the i comp onentofZ t equals and all the other comp onents equal In this waywe

k

obtain a total of M N equations of the form α k (t) 1

a a a

k-1 k k+1

Figure Shap e functions used in the numerical metho d

Z

a

k 

H H

i i

dt

k k

X

a X

k

The integral can b e split into twointegrals

Z Z

a a

k  k

H H H H

i i i i

dt dt

k k k k

X X

X X

a a

k k

These expressions can b e approximated using only the values of the unknown function

X t at the mesh p oints We approximate the integrals using the meanvalue theorem

and replace the derivatives X with nite dierence approximations

a a X a X a

k  k k k 

X

a a

k k 

If the equations for all the comp onents of X at p oint a are assembled into a vector we

k

obtain the following vector algebraic equation

a a X X X X h

k k  k k  k k 

H

X

h

a a X X X X

k k  k k  k k 

H

X

h

h a a X X X X

k k k k k k

H

X

h

X X X X a a

k k k k k k

H

X

h

H H

and resp ectively But to use the mean value theorem where H and H stand for

X

X

X

X

functions H and H must b e continuous While this is true for H function H could

X X

X X

exhibit a jump discontinuity at the corners p oints where X is discontinuous However

we can still argue that the approximation in is sucient There are only nitely many

corner p oints and if the mesh is dense enough wecanshow that the error we make in the

approximation is small

If the values X a are the unknowns the ab ove pro cedure yields M N equations

k

to compute them For the variational problem we also haveM b oundary conditions

either given as part of the problem or obtained from the transversality conditions At

least formallywethus have enough equations to compute all the unknowns The following

plausible argument shows why these equations should also b e sucient to compute all

the unknowns The variation Z t is piecewise smo oth and each of its comp onents can b e

P

N

z t As N increases this approximation can b e approximated with a sum Z

ij j i

j

arbitrarily accurate Equation is linear in every comp onentofZ t so any equation

that can b e obtained from this equation through the ab ove discretization will b e just a

linear combination of the equations of the form This indicates that if the original

integral equation completely sp ecies the extremal the algebraic equations sp ecify

its values at the chosen discrete p oints

The nal outcome of the ab ove pro cess will b e a set of MN nonlinear algebraic equa

tions Wechose the NewtonRaphson metho d to solve this system of equations but

any suitable metho d could b e used An imp ortant prop erty of the system of equations

is that any equation only dep ends on the values of the unknown variables at three consec

utive mesh p oints The linearized system that has to b e solved with the NewtonRaphson

metho d thus has a blo cktridiagonal structure and the computational cost of nding the

solution is signicantly reduced see Section

State dep endent equality constraints

Consider an optimal control problem with equality constraints Without loss of generality

we assume that inequality constraints are not present and that none of the equality con

straints explicitly dep ends on the input We also assume that the equality constraints are

indep endent In other words

g g

k

rank k

x x

n

but

g g

k

u u

m

As in the pro of of Lemma the optimal control problem is converted into the equivalent

problem of Bolza by dening

x X f X

u X g

The Hamiltonian is dened by

T T

H L

The matrix from Equation can b e therefore partitioned in the following way

H H

 

X X



H H

X X

X X



 

R

X X



X X

 

FG

when F G X and Y are vectors with the obvious understanding of the notation

XY

Clearly the lower part of the matrix R is identically which means that the problem is

singular

We conclude that equality constraints that only dep end on the state can not b e ad

joined to the Hamiltonian H directly One waytoovercome this problem is to replace the

constraint

g x t

i

with the equivalent constraint

n

X

g

i

x and g x tj g x x t

j i i

t

x

j

j

If the matrix R in is still singular we replacex with the system function f x u t

Equation and rep eat the same pro cedure until we arrive at an equation that explicitly

dep ends on the input For the motion planning problems considered in this work the

pro cedure always led to a regular problem

Another complication that arises with the equality constraints that do not explicitly

dep end on the inputs concerns the numb er of b oundary conditions that can b e sp ecied

for the problem If k constraints do not dep end on the inputs these constraints completely

determine k states as a function of the remaining n k states assuming that holds

Consequentlywe only have n k indep endentvariables and we can sp ecify at most n k

b oundary conditions On the other hand each time a constraint is dierentiated we

obtain an additional b oundary condition Clearly this new b oundary conditions must b e

consistent with the existing ones otherwise the problem will not have a solution

State dep endent inequality constraints

In Section wementioned that extremals for the problems with inequality constraints

typically con tain corner p oints At these p oints the extremals are not smo oth and the

Lagrange multipliers can b e discontinuous According to Corollary and Prop osition

the relation b etween the Lagrange multipliers corresp onding to inequality constraints

for the optimal control problem and the variational problem is given by

where is the multiplier for the optimal control problem and the equivalentmultiplier for

the problem of Bolza If has a jump discontinuity will also have a jump discontinuity

which means that is continuous Jump discontinuities in the multipliers thus get

eliminated through integration when the optimal control problem is transformed into the

equivalentvariational problem

But if the inequality constraints do not dep end on inputs the problem gets more

complicated Assume that there is a single inequality constraint

hx t

It is straight forward to generalize our discussion to more than one constraint Jacobson et

al showed that when such inequality constraint is directly adjoined to the Hamiltonian

of the optimal control problem with a multiplier Equation the integral of can

have jump discontinuity where the constraint b ecomes active or ceases to b e active Since

the integral of is equal to the later will b e discontinuous This violates the assumption

that the solutions of the variational problem are piecewise smo oth whichwas used in

derivation of the numerical metho d in Section

It app ears that this problem can b e avoided using an idea suggested by Jacobson and

p

Lele We assume that h C and that

i

p

d h

d h

p i

dt du

and for i p

u u

The number p indicating howmany times the constraint has to b e dierentiated b efore the

inputs explicitly app ear is called the order of the inequality constraint The inequality

constraint is rst transformed into an equality constraint

hx t

with a slackvariable Jacobson and Lele then to ok the sequence

h

d dh

dt dt

p p

d h d

p p

dt dt

and dened new state variables

y

y

p

d

y

p

p

dt

and a new input

p

d

v

p

dt

The initial values for the state variables y y can b e computed from the sequence

p

We will denote these initial values by y y We can therefore write the state

p

equations

y y y t y

y y y t y

y y y t y

p p p

p

y v y t y

p p

p

If the state variables y and the input v are substituted into the last equation in

i

the equation can b e rewritten as

p p

d h d

Gx u y v t

p p

dt dt

It is not dicult to see that Equation together with the state equations and initial

conditions is equivalent to the original inequality constraint Further the

equality constraint explicitly dep ends on the new input v It therefore app ears that

wewere able to avoid the problem of discontinuities of the unknown functions

Further analysis of the resulting system shows that the problem is still there Wecan

verify that y is given by

p

y h

Since y y it follows that

h

p

y

h

But on the constrained arc y is identically which implies that at p oint t where the

c

system switches b etween the unconstrained arc h and the constrained arc h the

values of y are

h

p

y j  y j lim

t t

c c

h

tt

c

Clearly in general the twovalues will not b e the same and y will have a discontinuity

The metho d by Jacobson and Lele can b e mo died to avoid the discontinuityofy

We replace the equation dening y in by

d dy

y

dt dt

With this denition the sequence of equations b ecomes

h y

dh

y

dt

p

d h

v

p

dt

so that

dh

y

dt

p

Since we assumed that h C IR y is continuous It is also clear from Equation

that all the other states y are continuous We can pro ceed in the same wayasaboveto

i

formulate the optimal control problem

Unfortunately this derivation hides yet another problem If we expand Equation

we obtain

y y y

This is clearly a singular dierential equation at y hence the optimal control problem

derived with the ab ove metho d is singular The singularity is not a pro duct of our metho d

and Jacobson and Lele p ointtoitin Intuitively the singularity is the result of the

nature of the state inequality constraint At the time the constraint b ecomes active all

higher derivatives of the slackvariable instantaneously b ecome The metho d of slack

variables therefore invariably leads to singular problems In numerical computations the

singularity results in oscillations of the slackvariable at the junction of constrained and

unconstrained arcs Because of the oscillations the convergence of the NewtonRaphson

metho d b ecomes quite slow

A direct metho d for solving the Bolza problem

The metho d from the previous section can deal with a general typ e of optimization problem

and is relatively easy to implement However as all indirect metho ds it can not distinguish

between minima maxima and inection p oints In other words even when the metho d

converges the critical p oint might not b e a lo cal minimum Furthermore if we determine

that the resulting critical p oint is not a minimum the only way to nd a dierent extremal

is to change the initial guess This is clearly not a go o d way of solving optimization

problems In this section wedevelop a direct metho d for solving the Bolza problem that

always converges to a lo cal minimum

As wesaw at the end of Section unconstrained formulation of the variational

problem dened in Prop osition can not b e a basis for an algorithm that directly

e to deal with the constraints in some other way There minimizes a cost functional Wehav

exists a numb er of techniques in nitedimensional nonlinear programming for dealing with

constraints Examples include p enalty function metho ds metho ds of feasible directions and

gradient pro jection metho ds The basic idea is to build a sequence of approximate

solutions that converges to the exact solution Most of the metho ds rely on the fact that

the space over which the function is minimized is closed and b ounded A closed and

b ounded subset of a nitedimensional space is compact which implies the existence of an

accumulation p oint for the sequence These metho ds can b e usually also formulated in

an abstract setting and subsequently applied to the innitedimen sional case However in

passing to an innitedimensi onal domain even if the domain is closed and b ounded it is

not necessarily compact The existence of an accumulation p oint and convergence of the

metho d is thus not assured

We follow an alternative approach to deal with the innitedimensi onal domain we

approximate the innitedimensi onal problem with a sequence of nitedimensional non

linear programming problems Formally one must show that the sequence of the solutions

of the approximate problems converges to the solution of the original problem sucient

conditions for this to b e true are derived in

We defer the discussion on how to approximate an innitedimensi onal problem with a

nitedimensional nonlinear programming problem until Section and rst concentrate

on metho ds for solving nitedimensional nonlinear programming problems

Penalty functions and augmented Lagrangian

Supp ose that wehave a nonlinear programming problem

min Lx sub ject to g x

x

where x b elongs to a subset of a nitedimensional vector space Toconvert this nonlinear

programming problem into an unconstrained problem we use the metho d of augmented

Lagrangian The metho d was indep endently prop osed by Hestenes and Powell

and combines the features of the p enalty function metho ds and Lagrangian metho ds

Itsmainadvantage is that it is relatively simple to implement and that it can handle

nonlinear constraints The main idea is to adjoin the constraint with a p enalty function

to the Lagrangian to obtain the augmentedLagrangian

c

g x H x Lxg x

c

It is not dicult to see that

inf lim H x inf Lx

c

x c x

An iterativescheme is obtained by exchanging inf and lim to compute

lim inf H x

c

c x

It can b e shown that under weak assumptions we can indeed exchange the minimization

and the limit pro cess We can then p erform a sequence of minimizations

min H x

c k

k

x

by using a nondecreasing sequence of p ositive p enalty parameters fc g Let x be a

k k

solution to this minimization problem for the value c If the Lagrange multiplier is xed

k

the p enalty parameter c has to b e increased to innity for the sequence x to converge

k

Further we can show that the minimization b ecomes to the solution of

increasingly illconditi oned as c increases However if after each step is augmented by

k

c g x

k k k k

where x is the solution of we can show that the metho d converges to the solution

k

of the original constrained problem for a nite value of the p enalty parameter c An

k

iteration consisting of a minimization step followed by augmentation of the Lagrange

multiplier thus represents a feasible numerical algorithm and can b e shown to have

good convergence prop erties

If wehave to satisfy an inequality constraint

hx

we can use exactly the same pro cedure after converting the inequality constrainttoan

equality constraint with a slackvariable

hx hx h

At each step we then have to minimize

c

hx min H x min Lxhx

c

x x

But for any given x the ab ove minimization can b e p erformed explicitly with resp ect to

By writing z wehave

c

min h z h z

z 

The ab ove function is quadratic in z with p ositive leading co ecient Its minimum z on

the p ositive halfplane is attained at

hxg z maxf

c

By writing

def

px c hxz maxfhx g

c

the cost function b ecomes

c

px c H x Lx px c

c

The up date rule for the Lagrange multiplier is therefore

c px c maxf c hx g

k k k k k k

An advantage of this derivation is that by using H x the slackvariable is completely

c

eliminated

In the case of twosided constraints of the form

hx

a similar pro cedure see pp for derivation leads to the following expression

for the augmented Lagrangian

H Lx phxc

c

where the p enalty function phxcisgiven by

c

hx if chx hx

c

hx if chx hx

phxc



otherwise

c

The corresp onding multiplier iteration is given by

c hx if c hx

k k k k k k

c hx if c hx

k k k k k k k

otherwise

The derivation is taken from

Minimization by Newtons metho d

Each iteration of the metho d of augmented Lagrangian requires minimization of a scalar

function Consider the following minimization problem

min f x

x

The general scheme for any iterative minimization metho d is to nd a direction of descent

d and up date the current solution by

k

x x d

k k k k

The most p opular metho ds include gradient metho d conjugategradient metho d New

tons metho d and quasiNewtons metho d The last three metho ds can achieve quadratic

convergence near the solution All these metho ds are globally convergent that is they will

converge to a lo cal minimum if one exists

Dierent metho ds dier in the wayhow the descent direction d is computed For

k

the Newtons metho d the descent direction is computed by scaling the gradient with the

inverse of the Hessian



d r f x rf x

k k k

The motivation for this choice of d is that it minimizes the secondorder Taylor expansion

k

of the function f x around x x Obviously the Hessian must b e invertible in order to

k

compute d Further d will b e a descent direction only if the Hessian is p ositivedenite

k k

A practical scheme that avoids these problems is

Mo died Newtons metho d Cho ose



d r f x rf x

k k k



if r f x exists and the following tests are satised

k

T  p

rf x r f x rf x c jrf x j c p

k k k k

 p



c jrf x j jr f x rf x j c p

k k k

Otherwise set

d D rf x

k k

where D is a p ositive denite scaling matrix and c c p and p are chosen constants in



c p p the suggested choice is c

The co ecient in Equation can b e chosen in a number of ways One p ossibility

k

is

arg min f x td

k k k

t

Another metho d which is often used in practice is the Armijo rule

i

Armijo rule Cho ose scalars s and and set a s where i

k

is the smallest nonnegativeinteger that satises

i i T

f x f x sd srf x d

k k k k k

It is shown in that the describ ed scheme for cho osing the descent direction d combined

k

with the Armijo rule for cho osing the co ecient results in a globally convergent metho d

k

Discretization

To approximate the innitedimensi onal continuous optimal control problem with a se

quence of nitedimensional nonlinear programming problems we employ collo cation tech

niques complemented by the ideas from the niteelement analysis Collo cation metho ds

treat the state variables and the inputs as unknowns and therefore pro duce larger systems

of equations than iterativeintegration schemes which eliminate the state variables

by substituting the function of the input obtained through the numerical integration of

the system equations However in collo cation the constraints can b e easily expressed and

we will demonstrate that it is easy to compute the gradient of the cost functional with re

sp ect to the unknowns In rob otic tasks state dep endent constraints are practically always

present in the form of kinematic closure equations or as b ounds on the range of motion of

individual joints Collo cation metho ds are thus well suited for discretizing motion planning

problems

Take the Bolza form of the motion planning problem

Z

t

min L x x dt

x

t

sub ject to

x x

g x

If the inequality constraints are present they can b e dealt with using metho ds from Section

The problem is discretized by assuming that the unknown vector function can b e

approximated with a set of basis functions

N

X

j

x t p t

i j

i

j

N

X

j

x t p t

i j

i

j

The approximation of the function is required to b e exact on the chosen set of grid p oints

t a a a a t

N  N

N

X

j

a p x a

j j i j

i

j

For simplicitywe assume that a a h for all j N

j j 

The approximating functions t form a subset of the basis for the space C t t and

j

are assumed to b e piecewise smo oth on the interval t t and smo oth on each subinterval

j

N

and letx be a a Let the values of the unknown x at the grid p oints b e fx g

j  j i

j

i

the collection of these values Further letx denote the approximation of the function x



If the time t explicitly o ccurs it can b e taken to b e an additional state variable so that the system

b ecomes autonomous

in Equation If the set f g is prop erly chosen there will b e a corresp ondence

j

j j

between the co ecients p and the values x and therefore b etweenx andx In this case

i i

the discretization of the unknown functions leads to the following approximation of the

optimization problem

Z

t

min L x xt dt

x

t

sub ject to

Z

a

j

x x dt j N

a

j

g xa j N

j

This is now a nitedimensional problem so we can use the augmented Lagrangian metho d

to deal with the constraints As a result wehave to solve a sequence of optimization

problems

x min H

c

x

where

Z

t

H L x x dt

c

t

Z Z

N

a a

X

j j

c

T

j

x x dt k x x dtk

a a

j j

j

N

X

c

T

j

kg xa k g xa

j j

j

and kk is the usual Euclidean norm

Choice of functions A necessary condition for solutions of the discretized problems

j

to converge to a solution of the continuous problem is that

lim x x

h

where the convergence is assumed to b e in the norm

kxk maxkxtk sup kx tk

S

t t

t t

In other words as the discretization gets ner functions must approximate x arbitrarily

j

well There are many p ossible choices for the appropriate set of functions As we shall

see b elow it is advantageous for the computation of the gradient if the shap e functions

have lo calized supp ort For our computations wecho ose the triangular shap e functions

describ ed by Equation and shown in Figure that were also used in the derivation

of the metho d in Section In this way the function x is approximated by a piecewise

linear function whilex is approximated by a piecewise constant function Figure

k k

One of the prop erties of the triangular shap e functions is that a where

j k

j j

j

is the Kronecker symb ol As a result the co ecients p in the approximation are

i

j j

exactly the functional values at the grid p oints p x

i i

Supp ort of a function is the set supp f Clfxjf x g where Cl stands for closure xi(t) (a) . (b) xi(t)

......

......

a0 a1 a2 aN−2 aN−1 aN t a0 a1 a2 aN−2 aN−1 aN t

Figure Approximation of the state a and its derivative b

Computation of the gradient rH

c

To simplify the computations we will assume that the supp ort of the approximating func

tions is a subset of the interval a a This is obviously the case for the triangular

j j  j

shap e functions We can rewrite H as

c

N

X

c c

j j j T j j j j T

F x G x G x H x I xF x

c

j

where wehave denoted

Z

a

j

j

L x x dt I x

a

j

Z

a

j

j

x x dt F x

a

j

j

G x g xa

j

j

Further let x denote the function under the summation sign in

c c

j j j T j j j T j j

x I x F x F x G x G x

The assumption that the supp ort of the function is a subset of a a implies that

j j  j

j

only dep ends on the values ofx at the grid p oints a and a Therefore

j  j

j j

H

c

j j j

x x x

i i i

We also have

T

j j j j n o

T

G F I c

j j j j

G cF

j j j j

x x x x

i i i i

j j j

I F G

Expressions and are matrices of partial derivatives These matrices can b e

j j j

x x x

i i i

j j

computed directly except for the derivatives of the integrals I and F We therefore fo cus

on dierentiation of a term of the form

Z

a

j

j

x x dt x

a

j

Changing the order of dierentiation and integration wehave

Z

j

a

k

dt

j j

j

x x

a

x i i

k

i

Since is in general nonlinear this integral can not b e evaluated analytically The rst

order approximation is obtained by using the midp oint rule for the integration

j j  j j j  j  j j j 

h x x x x x x x x

j

x h x h

x i i

i

j 

The rstorder approximation for is obtained in an analogous way

j

x

i

j j j j j j j j j

h x x x x x x x x

j

x h x h

x i i

i

Note the similarity of these expressions to the equation Derivation in this section

can thus b e used as an alternative to the derivation in Section

Computation of the Hessian r H

c

Wehave the expressions for the gradient rH For the Newtons metho d we also need to

c

compute the Hessian r H Because of the large numb er of unknowns it is not practical to

c

derive the analytical expressions for the entries of the Hessian We therefore use a simple

forward dierentiation scheme to compute these entries

j j

H xe H x

c c

i i

l l

H

x x

c

k k

j

l

x x

i

k

j

In the equation e represents a matrix that has zero es everywhere except in the comp onent

i

i j

Discussion

The main diculty in nding solutions of the optimal control problems is due to the

presence of corners that is p oints where the derivatives of the unknown function xt

exhibit jumps It can b e shown that at the corners the Lagrange multipliers can also

b ecome discontinuous Prop osition established that the minimum principle

is closely related to the rstorder necessary conditions in the form of EulerLagrange

equations Consequently corners will represent the same problem for the optimal control

formulation as for the Bolza formulation

Corners typically o ccur for problems with inequality constraints b ecause the dimension

of the manifold on which the system evolves changes as the system passes b etween the

unconstrained motion and the constrained motion If multiple constraints are present the

dimension of the manifold will change each time one of the constraints b ecomes activeor

ceases to b e active As the system passes b etween manifolds of dierent dimensions the

derivatives of the state typically change discontinuously

Indirect metho ds implicitly assume smo othness of the extremals and solve the equa

tions obtained from the rstorder necessary conditions These metho ds are therefore not

directly applicable to problems with inequality constraints Similarly direct metho ds in

tegrate the system equations and the adjoint equations to compute the gradient directions

of the cost functional and are therefore sensitive to corners to o In contrast discretization

metho ds solve a nitedimensional problem and problems with corners do not arise

We argued that the Bolza formulation is convenient for describing motion planning

problems Consequentlywe develop ed a numerical metho d that can solve the problem

in this form The conversion of the optimal control problem into a variational problem is

fairly formal and can b e easily automated In this conversion integrals of the inputs from

the optimal control problem b ecome the unknown functions These unknowns are thus

continuous enhancing the numerical stability of the metho d

The main disadvantage of the presented metho d is that it requires solving a sequence

of nonlinear programming problems We describ ed one p ossible technique for solving the

nonlinear programming problem to illustrate how a complete numerical algorithm for solv

ing the motion planning problem can b e implemented By cho osing a more sophisticated

metho d for dealing with constraints for example exact p enalty functions it is p ossible to

solve the nonlinear programming problem in one step The presented framework for dis

cretization of the continuous problem is thus indep endent of the metho d chosen for solving

the resulting nonlinear programming problem

Choice of the cost functional

Choice of the cost functional dep ends on the nature of the task for which the motion is

planned Quite often requirements of the task are contradictory and the cost functional

represents a tradeo b etween dierenttyp es of criteria An example is timeoptimal control

in rob otics while we wish to minimize the time for the rob ot motion we also lo ok for

smo oth actuator forces so that the stress on the actuators and the mechanism is reduced

ypically bang These two requirements are contradictory since the timeoptimal solution is t

bang In this example wehave to trade b etween minimizing the time and maximizing the

smo othness of the actuator forces see

In the intro duction we argued that metho ds based on minimizing a cost functional

are suitable for motion planning b ecause they provide a unied framework for resolving

kinematic and actuator redundancies But we also saw that in certain cases the solution of

the variational problem was purely kinematic for the cost functional while in other

instances the solution dep ended on the dynamic equations for cost functional We

would therefore like to establish what prop erties the cost functional must p ossess to yield

an unambiguous solution for the actuator forces

In this dissertation we are mainly concerned with motion planning for rigid mechanical

linkages The Lagrange equations for such systems can b e written in the form

T

I q q C q q Gq B q q

where q is the vector of generalized co ordinates of the system is the vector of actuator

forces the vector of constraint forces I q the inertia matrix C q q the vector of Coriolis

and centrip etal forces Gq the vector of gravity terms B q is the matrix that describ es

how the actuators act on the system and q is the matrix describing the constraints

q q

The inertia matrix I q is p ositive denite so we can rewrite the dynamic equations for a

rigid linkage in the state space form We dene

x q u

x q u

and rewrite Equation as

x x

 T

x I x C x x Gx B x u x u

Wemust also satisfy the constraint equations

x x

In Chapter wesaw that when the optimal control problem is reformulated as a varia

tional problem the actuator inputs b ecome additional unknowns that have to b e computed

as part of the optimization Wementioned in Section that the EulerLagrange equa

tions for regular problems can b e rewritten as a system of rst order dierential equations

in the unknown functions and so called canonical variables and in general will yield

an extremal For regular problems the actuator forces can b e therefore computed from

the EulerLagrange equations

In many cases the system function f dep ends on the inputs linearly When there are

no additional equality or inequality constraints and the cost functional is dened as

Z

t

T

J u udt

t

then H I where I is the m m identity matrix and the problem is regular

uu

This is one of the reasons that the cost functional is p opular in the optimal control

community The inputs in rob otic systems are actuator forces and the cost functional

is often taken for a measure of energy consumed during the motion The intuition

b ehind suchinterpretation is that torques pro duced with an electromechanical actuator

are approximately prop ortional to the currentowing through the motor and that the rate

of energy consumption is approximately prop ortional to the square of the current

The minimum torquechange criterion

Z

t

T

dt J

t

used byUnoet al tomodelhuman arm motion also has the form of if we

dene

u

and extend the state of the system with the actuator torques On the other hand for

the minimumjerk criterion

Z

t

T

x x

dt J

t

where x is the p osition vector the state equations can b e dened as

x x x x

x x x x

x x x u

This is a purely kinematic problem and is singular if the inputs are the actuator forces

Therefore it can not b e used to compute the actuator tra jectory

It app ears that also timeoptimal control problems are singular since the cost functional

do es not dep end on the inputs

Z

t

dt J

t

However for the timeoptimal control wehave to satisfy the constraints

U u U

min max

which b ecome part of the side constraints It is not dicult to check that b ecause of the

constraints this problem is regular

In many motion planning problems it is desirable to obtain motion plans that are

smo oth Smo oth tra jectories prevent jerky motions that can excite structural resonances

of the system and damage the mechanical structure or the actuators It is customary to

takethe L norm of the derivativeofacurve for a measure of its smo othness At rst it

norm of higher derivatives should pro duce a curve that is seems that minimizing the L

even smo other The following simple example shows that such conclusion would b e wrong

Supp ose we are interested in smo oth curves in IR between p oints and It

is not dicult to see that the curve xt that minimizes

Z

J x dt

is the straight line x t tWewould like to see which curve minimizes the integral

Z

J x dt

On the straight line x tt the value of this cost functional is and this is the global

minimum Now supp ose that the initial and nal velo cities for the curve are sp ecied to

b e In this case the curve that minimizes J is the cubic x t t t Because

we imp osed additional b oundary conditions on the curve the value of the cost functional

J increased Of course also the value of J for the curve x t is higher than the value

for the curve x t This example illustrates that it would b e wrong to take L norm

of higher derivatives of a curve as a measure of smo othness In deciding what order of

derivatives to use as a measure of the smo othness of a curve wemust b e guided by the

b oundary conditions that the curve has to match in general wemust also consider other

constraints and use the derivatives of the smallest p ossible order

Minimum energy minimum torquechange and minimumjerk func

tionals can all b e interpreted as measures of smo othness the rst two of the actuator

tra jectories and the last of the kinematic tra jectories As demonstrated ab ove the rst

two criteria are suitable for dynamic motion planning while the last can only b e used for

kinematic motion planning According to the ab ove example the criterion for measuring

the smo othness should b e inferred from the b oundary conditions that need to b e satised

For rob otic systems we often want to assure that the actuator torques do not exhibit jumps

as we switch from one motion to another This is the same as saying that the accelerations

at the junction p oints of dierent arcs are the same In general if a b oundary condition

foravariable needs to b e sp ecied the cost functional should dep end on the derivatives of

the variable If we need to sp ecify the accelerations the cost functional should either de

p end on the derivative of the acceleration or if we wish to use the dynamics the derivative

of the torques Minimumjerk and minimum torquechange functionals dep end on these

derivatives hence they can b e very useful in rob otics Which of the twowe pick dep ends

on whether weareinterested in kinematic or dynamic motion planning

Chapter

Motion planning for two

co op erating rob ots

In this chapter we study two manipulators co op eratively manipulating an ob ject We

wish to nd smo oth tra jectories for the actuator torques hence the minimum torque

change cost functional is used to formulate the motion planning problem Motion plans

are computed for two cases When the ob ject is rigidly grasp ed the manipulators can

exert arbitrary grasp forces and the motion planning problem is unconstrained But when

the two manipulators hold the ob ject with a friction assisted grasp frictional inequality

constraints that only dep end on the state must b e satised

Dynamic mo del for two manipulators holding an ob ject

We start by deriving the dynamic mo del for two manipulators holding a rigid ob ject Both

manipulators are assumed to have n links and n actuated degrees of freedom but we

could easily extend the derivations to more general case The interaction b etween each

manipulator and the ob ject is assumed to b e rigid no relative motion b etween the ob ject

and the manipulator is allowed All degrees of freedom in the system thus corresp ond to

the degrees of freedom of the two manipulators The motion of the ob ject is describ ed in

a mdimensional conguration space where m for the planar case and m inthe

spatial case See Figure for a case when n and m

Dynamics of the two manipulators is describ ed by the following equations

T

G F I C

T

I C G F

where is the n vector of the joint co ordinates of the ith i manipulator I

i i i

n vector of nonlinear terms Coriolis and is the n n inertia matrix C isthe

i i i

centrifugal forces G isthe n vector of gravity terms is the n vector of the

i i i

joint torques is the n m Jacobian matrix relating the velo city of the center of mass

i i

of the ob ject in the conguration space to the jointvelo cities of the manipulator i and F

i

is the m generalized force vector in the conguration space consisting of the forces and

the moments ab out the center of mass exerted on the ob ject by the manipulator i

The dynamics of the ob ject in the conguration space is given by

M p F F

where M is the m m inertia matrix of the ob ject and p is the m vector representing

the p ositionorientation of the ob ject in the conguration space

The two manipulators completely restrain the motion of the ob ject forming a closed

kinematic chain The kinematic closure equations can b e obtained by expressing the p osi

tion of the center of mass of the ob ject which is xed with resp ect to the rob ot endeector

as a function of the joint co ordinates of each rob ot If p is the p osition vector of the

center of mass of the ob ject expressed as a function of the joint co ordinates of the rst

rob ot and p the p osition vector of the center of mass of the ob ject expressed as a

function of the joint co ordinates of the second rob ot then the kinematic closure equations

can b e written as

p p

By dening

g p p

it is easy to see that

g

g

The closed kinematic chain has n m degrees of freedom If r degrees of freedom are

required to p erform the task the task can b e p erformed as long as r n m If strict

inequality holds the linkage is kinematically redundant for the given task There are also

n actuators at our disp osal for moving the joints The system is therefore overconstrained

and m actuators are redundant

In dynamics the interaction forces F and F due to the holonomic constraint

are also called the Lagrange multipliers The fact that the closed chain is overconstrained

and p ossesses m surplus actuators implies that the interaction forces that do not aect

motion of the linkage can b e exerted on the ob ject Such forces are called the internal

or preload forces and are typical in tasks suchaswalking grasping and multiplearm

co ordination during which closed kinematic chains are formed Internal forces can b e

understo o d through an example where two force sources act on a rigid ob ject Figure

For the case of manipulation with two arms the two sources are the two arms Assume

that the ob ject is a p oint mass and that the two sources exert forces F and F on the

ob ject The ob ject moves according to the equation

mp F F

where m and p denote the mass and the p osition of the ob ject resp ectivelyNow assume

 

that the exerted forces are changed to F F andF F Obviously the sum

of the forces do es not change

 

F F F F

so the motion of the ob ject will not b e aected by the force The force is thus an

internal force Assuming that each source contributes the same share to the motion of the

ob ject we can write

F

F

F

F

and the internal force is given by

F F

When wehave n actuators op erating in a linkage with r degrees of freedom r n the set

of internal forces is in general a vector space of dimension n r

F2

F1

Figure Internal force in an overactuated system

In rob otics kinematic and actuator redundancies are typically resolved with some sort

of lo cal optimization The advantage of suchschemes is that they can b e used

online as the motion is executed However they require a separate tra jectory planning

phase Instead bycho osing a suitable cost functional and solving the resulting variational

problem tra jectory planning and redundancy resolution are p erformed at the same time

The resulting motion plan is globally optimal and can b e subsequently used to simplify

the control

Motion planning for two planar co op erating arms

We turn our attention to the case of twoR manipulators moving an ob ject in the hori

zontal plane as shown in Figure Each manipulator has three actuators n The

ob ject moves in the plane and m hence the system has n m degrees of free

dom The task is to arbitrary p osition and orient the ob ject in the plane Three degrees

of freedom are required to p erform the task so r andwehave no kinematic redun

dancy All joints of the rob ots are actuated yielding a total of actuators In general

only are required to move the linkage whichleaves surplus actuators and gives rise to

a threedimensional space of internal forces

Since the closed kinematic chain is not kinematically redundant the matrices and

in equations are invertible and the forces F and F can b e expressed from Equation

In the planar case these forces have the form (x,y, φ ) θ θ 6 3 θ 5

θ l 2 l 2 5

F1t F 2n Robot 2 l l 4 y 1 Robot 1 F1n F θ θ 2t 4 1 x Object

l 0

Figure Two planar R arms holding an ob ject

F

i

x

F F

i i

y

T

where F and F are the x and y comp onents of the Cartesian force and T is the moment

i i

x y

that the contact force pro duces around the center of mass of the ob ject Similarly the

ob ject inertia matrix b ecomes

m

M m

I

c

where m is the mass of the ob ject and I the moment of inertia of the b o dy around the

c

center of mass By replacing the forces F and F in Equation with the expressions

obtained from Equation and expressing the p osition of the center of mass of the

ob ject as a function of the jointvariables of the rst rob ot p p a single equation

describing the dynamics of the closed chain is obtained

T T

M I C I C

We neglected the gravity terms given that the system moves in the horizontal plane

Next we dene the cost functional We consider the task of moving the ob ject from

an initial p osition to a nal p osition along smo oth tra jectories so that the initial and nal

accelerations are As discussed in Section to satisfy b oundary conditions on the

acceleration the cost functional must dep end on the derivative of the p osition variables of

the order or more One p ossible choice is the minimumjerk functional

Z

t

T

p p

dt J

t

The vector p denotes the p osition of the ob ject in the taskspace Analytic expressions for

the kinematic tra jectories that minimize the minimumjerk cost functional can b e readily

calculated and are fthorder p olynomials

The system of the two manipulators holding an ob ject is overactuated Therefore

nding the kinematic tra jectories is not enough to uniquely determine the actuator forces

To compute the actuator forces through the optimization wehave to use lo osely sp eaking

a dynamical equivalent of the minimumjerk cost functional In this waywe obtain the

minimum torquechange functional

Z

t

T

dt J

t

where is the vector of the actuator forces see Equation Since the equation for the

acceleration dep ends on the minimum torquechange functional can b e intuitively un

dersto o d as replacing the jerk with to make the cost functional dep end on the dynamics

We therefore require smo othness of the actuator forces and not directly smo othness of the

tra jectory of the ob ject

No constraints on the interaction forces

When no constraints are imp osed on the interaction forces F the optimal control problem

i

is completely dened by the dynamic equations constraints and the cost

functional However the dynamic equations have to b e rewritten as a system

of rst order dierential equations Weintro duce a vector of unknowns

X

X

which implies

X X

We further dene

X

With this denitions Equation can b e rewritten as

I X X C X X B X X

where

i h

T T

I I I X M

T T

C X X M C C

h i

T T

B X

With the denition the cost functional b ecomes

Z

t

T

X dt X J

t

Note that the matrix I in Equation is not a square matrix and is therefore not

invertible However for the Bolza formulation of the optimal control problem the dynamic

equations need not b e explicit so we can directly apply the metho ds from Chapter

Finally as discussed in Section the holonomic constraints

g

must b e dierentiated to obtain a regular problem After dierentiation and using Equation

the constraint b ecomes

In addition wehave to enforce the initial condition

g j

t

The optimal control problem is not completely sp ecied without the b oundary con

ditions The initial and the nal p ositions of the ob ject are sp ecied Furthermore we

require that the ob ject is at rest at the b eginning and end of the motion Therefore

X t X X t

X t X X t

For the manipulation task we require that the initial and nal accelerations of the ob ject

are Hence there are three ways to sp ecify the b oundary conditions for the torques

The actuator forces at the initial and the nal time are arbitrarily sp ecied sub ject

to the requirement that the initial and the nal accelerations are

The internal force at the initial time is sp ecied while the ob jects initial acceleration

is The ob jects acceleration at the nal time must b e but the internal force is

not sp ecied Alternatively the acceleration at the initial time could b e set to zero

and the forces and the accelerations sp ecied at the nal time

The acceleration of the ob ject at the initial and at the nal time must b e No

internal forces are sp ecied

The rst case is the easiest to implement the other two cases require some additional

work to determine the transversality conditions see pp When the b oundary

values for the actuator forces are not directly sp ecied they are computed from these

transversality conditions as part of the optimization

Wehave not sp ecied whether the nal time t is given in advance or has to b e com

puted For simplicitytake t and assume that we computed the extremal for the nal

time t Supp ose wewant to compute the extremal for the nal time t where t t It

can b e veried that if t t t and t satisfy the side constraints and the b ound

then t t tt and t t ary conditions on the interval t

is a solution on the interval t Further if

Z

t

T

dt J

where is the input computed for the extremal the value of the cost functional for the

new time t t is

Z

t

T

J d t

Z

t

T

dt J

Therefore as the time t increases the cost decreases with factor It follows

that

lim J

t 

In summary the nal time t has to b e xed for the problem to b e wellp osed For any

other nal time t the solution can b e obtained by appropriate scaling of the solution for

the nal time t

tisan Another interesting observation that follows from the ab ove is that if X

extremal for the b oundary values X t X and X t X then the extremal for the

X and X t X is given by X t t This can b e useful b oundary values X t

in verifying the correctness of the numerical solutions

Frictional constraints on the interaction forces

The situation where the ob ject is completely restrained by the two rob ots so that arbitrary

forces can b e exerted on the ob ject seldom o ccurs in the co op erating task If the ob ject

is to b e grasp ed rigidly sp ecial gripp ers have to used and such gripp ers can not grasp

ob jects of arbitrary shap e Instead it is more common to restrain the motion of the ob ject

with frictional forces With such grasp the normal forces must b e p ositive and they must

pro duce suciently large frictional forces to completely immobilize the ob ject

We p erform the analysis of the frictional contact in the contact frame The origin of this

frame corresp onds to the p ointofcontact of the rob ot with the ob ject or the centroid of

the contact area if this area is not a p oint the x axis corresp onds to the inward normal to

the surface of the ob ject and the y axis to the tangent The contact force F is transformed

i

c

to the force F in the contact frame according to

i

c

F R F

i i i

where the screw transformation R transforms the forces from the co ordinate

i

frame at the cen ter of mass of the ob ject with the axes parallel to the inertial frame

to the contact frame at the contact p oint iContact forces are computed from Equation

T

F I C

i i i i i i i i

i

c c

We denote the normal and tangential comp onents of the contact force F by F and

i i

n

c

resp ectively F

i

t

c

F

i

n

c

c

F F

i i

t

c

T

Normal force must b e p ositive in order to pro duce the frictional force The rst constraint

is therefore

c

F

i

n

Tangential forces are frictional forces If is the co ecient of friction the tangential forces

hence satisfy

c

jF j F

i

n t

The last inequality can b e rewritten as a pair of constraints

c c

F F F F

i i i i

t n n t

We can rewrite the frictional contact constraints in a matrix form

c

D F

i

where D is a constant matrix

D

Therefore in their nal form the inequality constraints describing a frictional contact are

T

DR I C i

i i i i i i i i

i

Note that b ecause of the choice of the cost functional the torques are part of the state

and the inequality constraints do not dep end explicitly on the inputs

Numerical results

To solve the optimal control problem wefollow the metho d from Section In this

example the metho d pro duced a lo cal minimum we established this bychecking the

second order necessary conditions so we did not use the more sophisticated metho d from

Section We dene the Hamiltonian H according to Equation and Prop osition

Subsequently a system of algebraic equations of the form is obtained The

vector of unknown functions X has dimension The dynamic equations

and the constraints dene side constraints which are adjoined to H with

ultipliers A total of unknown functions must b e therefore computed to obtain an m

extremal

When the contact forces are constrained we pro ceed as b efore except that the con

straints are transformed into equality constraints with the slackvariables and ad

joined to the Hamiltonian with the Lagrange multipliers The numb er of unknown func

tions when the inequality frictional constraints havetobetaken into accountthus increases

to

Any suitable numerical metho d can b e used to nd a solution of the system of algebraic

equations We decided to use the NewtonRaphson metho d The idea of the metho d

c   

In the spatial case we can write F F

i i

t n

is quite simple Wewant to nd a zero of the vector function F x If x is the current

approximation of the solution we are lo oking for a correction x such that

F x x

The rstorder Taylor expansion yields

F

x F x x F x

x

x

which implies that a go o d approximation to the desired vector of corrections is



F

x F x

x

x

F

Since In each iteration wehave to compute and invert the matrix of gradients E

x

the numb er of unknowns dimension of the system of algebraic equations is MN where

M is the numb er of unknown functions and N the numb er of mesh p oints inversion of

the matrix E is a time consuming pro cess However Equation only dep ends on the

values of the unknown functions at three adjacent mesh p oints Matrix E is therefore

blo cktridiagonal and can b e inverted in time O NM instead of O N M

The second simplication in our numerical scheme involves computation of the entries of

E This entries should b e computed analyticallyFor any problem of practical signicance

Equations are already quite complicated and computation of the partial derivatives

would b e a lab orious pro cess The entries in E are therefore computed numerically using

forward dierence scheme

For the simulation the two manipulators were assumed identical and they were mo deled

by the human arm The dimensions of the manipulators were

Link Length Mass Inertia



m kg kg m

The ob ject was a rectangle of length m width m mass kg and moment of inertia



kg m The co ecientoffrictionwas when frictional constraints were

taken into account Duration of motion was s

As discussed in Section b oundary conditions for the actuator forces can b e de

termined in dierentways For numerical simulations we sp ecied the initial preload

compressive force and enforced zero acceleration at the b eginning and end of the motion

case on page

Figure compares the tra jectories of the unconstrained motion dotted with two

constrained motions one with the normal preload force of N solid and the other N

dashed The gure shows that the tra jectory is curved for the unconstrained case and

it is almost a straight line for the two constrained cases Varying the preload force only

slightly changes the tra jectory 0.15 0.1N 1 4 2N unc. 0.1 0.5 3 1

0.05 0 y [m] 6 Torque [Nm] -0.5 2 0 5

-1 -0.05 -0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0 0.2 0.4 0.6 0.8 1

x [m] Time [s]

Figure Tra jectories of the center of Figure Optimal distribution of the

mass of the ob ject torques for the preload force of N

The calculated optimal distribution of the torques for the constrained case with the

preload force of N is shown in Figure The numb ers denote the joints of the rob ots

the rst three corresp onding to the rst rob ot and the last three to the second rob ot The

gure indicates that b ecause of the constraints on the contact forces the torques do not

change smo othly at the junctions of constrained and unconstrained arcs compare with

Figure The inputs torque derivatives are continuous but not smo oth Analysis

of the normal forces not presented on the gure shows that the constraint never

b ecomes active the normal force is always strictly greater than

0.75 0.75

Left Left 0.5 Right 0.5 Right

0.25 0.25

0 0 Ratio Ratio

-0.25 -0.25

-0.5 -0.5

-0.75 -0.75 0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1

Time [s] Time [s]

Figure Ratio of the tangential vs Figure Ratio of the tangential vs

normal force for the preload force of N normal force for the preload force of N

Figures and show the ratios of the frictional and normal forces for the two

constrained motions The ratios clearly show when the constraints are active On Figure

we can see that the tangential force at the rightcontact has two constrained p ortions

with unconstrained parts at b oth endp oints and in the middle There are four p oints at

which the unconstrained and the constrained arcs are joined together implying that there

are corner p oints Similar analysis shows that additional corner p oints o ccur b ecause

of the frictional constraint for the left arm We can also see that b oth arms simultaneously

move along constrained arcs for a p erio d of time in the rst half of motion The system as a

whole thus exhibits quite complicated b ehavior as the number of active constraints changes

throughout the motion Figure shows that the b ehavior of the system changes when

the preload force increases Increased preload force results in a considerable normal force

giving more ro om to the frictional comp onent Consequently there are fewer regions

where the constraints are active and eachtangential contact force only saturates once

The comparison of the two constrained motions also shows that it is dicult to predict

the structure of the solution and the numberofcornerpoints

Discussion

We studied the example of two arms manipulating an ob ject through frictional p oint

contacts The task was to nd a dynamically smo oth tra jectory for the system while

optimally distributing forces b etween the actuators To satisfy b oundary conditions on

the accelerations inputs to the system had to b e the derivatives of the actuator forces As

a result inequalities representing the frictional constraints only dep ended on the state

This example is representative of tasks suchasmultingered grasping multiarm ma

nipulation walking and running In the past such problems havebeentypically solved

using lo cal optimization schemes in which the task space tra jectory is determined inde

p endently often in an ad hoc fashion In contrast our metho d provides globally optimal

solutions for the force distribution actuator forces as well as for the joint tra jectories

Several advantages of solving the optimal control problem byconverting it into a vari

ational calculus problem are apparent from the example The variational formulation

allowed us to use Equation directly without rewriting it in the statespace form

Such implicit dierential equations are obtained whenever the Lagrange multipliers are

eliminated from the dynamic equations To transform the system into the statespace

form wewould also have to eliminate the extra state variables using the kinematic clo

sure equations Such elimination is not feasible unless the rob ot kinematics is analytically

invertible

In the variational calculus problems Hamiltonian is allowed to dep end on the deriva

tives of the unknowns For this reason we did not eliminate the accelerations and

from the constraint equations This again simplies the formulation of the problem

We conclude this chapter with a brief note ab out the computational eciency of the

numerical metho d For quite arbitrary initial guess straight lines b etween the b oundary

values where these are known zero functions otherwise ab out steps of NewtonRaphson



The number of metho d were p erformed to obtain the solution with the accuracy

iterations did not vary with the numb er of discretization p oints For p oints each

NewtonRaphson iteration involved calculating the co ecients and solving a system of

linear equations This computation takes approximately s on Silicon

Graphics Indigo

Chapter

Mo deling of human twoarm

motion planning

Motion planning for rob otic systems and mo deling of principles that govern motion plan

ning in humans are complementary problems in the rst case we try to generate the

tra jectories for the system while in the second case we measure the tra jectories and try to

discover how they were generated A principle that governs human tra jectory generation

can b e used for rob ot tra jectory planning In this chapter we pro ceed in the other direc

tion we try to mo del human tra jectory generation using principles that were develop ed

for rob ot motion planning

Based on studies of single arm reaching in humans Flash and Hogan suggested

that the central nervous system uses an optimality criterion to plan the tra jectory They

observed that medium sp eed large amplitude unconstrained planar motions for a single

arm can b e well describ ed with tra jectories minimizing the integral of the jerk These

minimumjerk tra jectories dep end only on the kinematics of the task Kawato et al

prop osed an alternative cost functional to mo del human tra jectory generation

the integral of the norm of the vector of derivatives of the actuator torques This cost

functional called the minimum torquechange criterion takes into account the dynamics

of the system According to the studies of co ordinated manipulation with two arms by

humans the success of the minimumjerk mo del in accounting for twoarm tra jectories

is limited In this chapter weinvestigate if the minimum torquechange criterion b etter

describ es tra jectories for human twoarm manipulation We present exp erimental obser

vations for the task of p ositioning and orien ting an ob ject rmly grasp ed bytwo hands

Three typ es of motions are analyzed and it is demonstrated that during these motions the

force distribution b etween the two arms is asymmetric and dep ends on the conguration

The mo del develop ed in the previous chapter for planning the motion for two co op erating

manipulators is prop osed for mo deling of the data Because of the asymmetry of the force

distribution the mo del is only applied when the manipulation task is symmetric for the

two arms For such motions kinematic features of the tra jectories and force distribution

between the two arms observed in humans are well predicted by the mo del

Exp erimental results

Twoarm planar motions of human sub jects were recorded using the three degree of freedom

planar passive manipulandum shown in Figure The apparatus consists of three links

connected by revolute joints The third link is a handlebar that can rotate around its center

Three optical enco ders mountedateachjoint are used to record the manipulandum joint

angles Six axis force sensors are mounted underneath each handle allowing measurement

of the forces and torques exerted on the handles by the sub ject The enco ders and the force sensors are sampled at Hz

a d LED Target 4 a b c 0.110 m 4 23 a a 1 2 3 y b b Ο x c c 0.085 m 1 L R a b R φ c L R b LED L C L R c

0.150 m 0.150 m

Figure Exp erimental setup

The sub ject is seated in front of a transparent Plexiglas plate and rmly grasps the

two handles of the handlebar Figure a The plate lies horizontally at the level of the

sub jects chin Four target sets are mounted on the plate as shown in Figure d Target

is cm in front of the sub jects chest Target Target is displaced bycm forward

and cm to the left right of Target and Target is displaced cm forward with

resp ect to Target Each target set consists of arrays of light emitting dio des LEDs

that can sp ecify target p ositions and orientations Figure c For detailed description

of the exp erimental setup we refer the reader to

During the exp eriment the ro om was darkened and a computer generated sequence of

target congurations was displayed Sub jects were instructed to move the handlebar at

their preferred velo city to the p osition and orientation sp ecied by the lit targets They

were instructed to prevent slippage b etween the hands and the handles and to keep the

elb ows in the shoulder plane so that a corresp ondence b etween the manipulandum

joint angles and the shoulder elb ow and wrist angles of the human arms was established

In all the exp eriments rep orted here the sub jects were instructed to keep the handlebar

parallel to the frontal plane Four sub jects participated in the study two righthanded

one lefthanded and one ambidextrous Three typ es of motions were studied a motions

parallel to the frontal plane and in Figure a b motions in the sagittal

plane and and c oblique motions and Velo cities and forces

exerted on the handles by the left and the right arm were studied for each motion

All quantities are expressed in a co ordinate frame lo cated at the rst target with the

xaxis normal to the sagittal plane and p ointing to the left and y axis normal to the frontal

plane and p ointing forward Only those comp onents for which the prescrib ed amplitudes

were nonzero are considered eg the y comp onents of motions parallel to the frontal

plane are disregarded These comp onents will b e referred to as signicant comp onents

Because of the large numb er of measured tra jectories we only present some representative

plots to give an idea of the exp eriments and the data that was collected

General observations

Velo city proles Some examples of normalized velo city proles are shown in Figure

for dierent sub jects Only the signicant comp onents are shown The velo city proles

are b ellshap ed but the shap e is not symmetric and the p eak o ccurs on average at around

of the duration of motion The rise to the p eak is steep er than the fall to zero Since

the sub jects did not have visual endp oint feedback of the handle it is not clear whether

this eect can b e attributed to a slowing down in order to achieve accurate p ositioning

1 a

0.8

0.6

0.4

Normalized velocity 0.2

0

0 0.2 0.4 0.6 0.8 1

Normalized time

Figure Sample plots of normalized velo city proles measured during the exp eriments

Force proles Some representative plots of the normalized signicant comp onents of

the total force acting on the handle are shown in Figure a The forces exerted on

the ob ject are roughly sinusoidal The force prole for a particular sub ject is in general

invariant with resp ect to the direction of motion but it varies from p erson to p erson

In part this dierence can b e attributed to dierent sp eeds at which dierent sub jects

p erformed the motions At higher sp eeds solid line in Figure a the prole is roughly

symmetric with the p eak and the following valley having approximately the same duration

and amplitude Atlower sp eeds the p eak was considerably sharp er and higher than the

valley

Force distribution Representative tra jectories showing the axial comp onent of the

internal forces F are shown in Figure b During motions with comp onents in the x

a

direction the internal force comp onentwas nonzero In other words the distribution of

forces b etween the two arms in the x direction is asymmetric In contrast the arm forces

in the y direction were roughly equal 6 1 b c 4 0.6 2 0.2 0

-0.2 Force [N] -2 Normalized force -0.6 -4

-1 -6 0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1

Normalized time Normalized time

Figure Sample plots of the measured forces a Total force acting on the ob ject b

Internal forces in the axial direction

Remark Some of these observations are a natural consequence of the physics of the

task and are not surprising If the ob ject is at rest at the b eginning and at the end of

motion and the manipulation task is otherwise unconstrained one can exp ect to see a

b ellshap ed velo city prole The shap e of the force prole is linked to the shap e of the

velo city prole b ecause the total force is prop ortional to the acceleration If the velo cityis

b ellshap ed the acceleration will b e sinusoidal The symmetry of the force distribution in

the y direction can also b e explained The sub jects did not exert large torsional moments

at the handles which implies that it is not p ossible to exert internal forces with signicant

comp onents in the y direction

Motions in the sagittal plane

The Cartesian tra jectories of the ob ject for motions that are nominally in the sagittal

plane are shown in Figure a Inward tra jectories solid are shown shifted to the left so

that they can b e compared to the outward tra jectories dotted An interesting observation

is that the tra jectories are always curved Further outward tra jectories curvein

the opp osite direction as inward tra jectories For b oth left and righthanded

sub jects outward tra jectories curve to the right while inward tra jectories curve to the

left The corresp onding internal forces axial comp onent only are shown in Figure b

The axial comp onent of the internal forces aects the forces in the x direction When the

sub jects moved inwards they exerted compressive forces p ositiveinternal forces on the

handle while on the wayoutwards the handle was in tension negativeinternal forces

This app ears to b e due to the natural tendency of the arms to move closer together on the

in ward path while on the way outward they tend to move apart Analysis of the individual

arm forces not shown indicates that in all cases the force exerted by the right arm had

slightly higher amplitude this was also true for the lefthanded sub ject Thus for the

inward motion when the arms compress the handle we can exp ect the tra jectory to curve a b

2N

0.05m

Trajectories Internal forces vs. normalized time

Figure Motions in the sagittal plane a Tra jectories for inward solid and outward

dotted motions b Internal forces in the axial direction

toward the left as corrob orated by our observations A similar argument can b e made for

the outward motion

Other motions

For motions parallel to the frontal plane and oblique motions the amplitude of motion in

the x direction is large Common to all motions with large amplitude in the x direction

is the asymmetry of the forces in the x direction and presence of relatively large internal

forces the axial comp onent Figure b shows that the internal forces tend to start at

zero b ecome compressive p ositive for a short p erio d and are afterwards tensile negative

This b ehavior can b e observed for left and righthanded sub jects Hence there is no ground

to b elieve that the dominance of one arm over the other plays an imp ortant role in the

distribution of forces It is also interesting that the total force exerted on the ob ject do es

not vary much from sub ject to sub ject but the distribution of the forces b etween the two

arms varies greatly

Optimal forces and tra jectories predicted by the mo del

Human twoarm manipulation task is mo deled bytwoR planar manipulators holding a

rigid ob ject Motion planning for this system was studied in Chapter During the exp er

iments the sub jects rmly grasp ed the handle of the manipulandum No constraints on

the contact forces are thus assumed in the mo del this case was describ ed in Section

Wehave seen in the previous section that the force distribution in the x direction varies

from sub ject to sub ject Only for the motions in the sagittal plane the variability is small

so we limit the comparison of the mo del with the exp erimental data only to this case We

chose a t ypical data set for a sagittal plane motion From this data set we obtained

the b oundary values for the optimization and then calculated the minimum torquechange

tra jectories The physical dimensions of the arms for the optimization were taken from the

measurements of the sub ject Dynamic parameters mass and moments of inertia of the

human arm were calculated from the normalized anthrop ometric measurements rep orted

by Winter

Vx

0.1m/s

Vy a

Velocity vs. normalized time

Figure Comparison of the measured velo city proles solid with the velo city prole

predicted by the mo del dashed

Figure compares the velo city proles predicted by the simulation dashed with

the measured ones solid As exp ected the simulation predicts that the velo city in the

x direction will b e close to It is slightly curved b ecause the measured initial and nal

p oints did not have the same x co ordinate The measured x velo city prole on the other

hand considerably deviates from b ecause the measured tra jectories are curved Figure

a The predicted and observed y velo city proles have similar shap es but there is also

some discrepancy b etween the two the measured tra jectory reaches the valley at

of movement duration while the computed tra jectory attains its minimum at This

disparity is also reected in the amplitudes

The comparison of the measured forces with those predicted by the simulation is shown

in Figs a and b The agreementbetween the simulated and the measured forces is

reasonable except for the force exerted by the right arm in the x direction where it is p o or

Figure b This discrepancy is due to an apparent dominance of the right arm over

the left arm whichwas observed for b oth left and righthanded sub jects The optimality

criterion on the other hand do es not incorp orate such dominance The discrepancy in the

y force comp onent is consistent with the dierences of the velo city proles in the y direction

The measured force prole exhibits a valley that is sharp er than the ensuing p eak The

two extrema the valley and the p eak are also not symmetric ab out the midp oint In

contrast the optimization results are fairly symmetric

Comparison of the computed and measured tra jectories shows that the minim um

torquechange mo del correctly predicts some features of the motion at least when the

eects causing asymmetric distribution of the load b etween the two arms are small These Fx Fx

1N 1N

Fy Fy b c

Left force vs. normalized time Right force vs. normalized time

Figure Comparison of the measured forces solid with the predictions of the mo del

dashed a Leftarm forces b Rightarm forces

features include the general shap e of the tra jectories velo cities and force distributions

While wehave only presented results for one instance of motion this set of data is typical

of what wehave observed with other sub jects as well On the other hand there are imp or

tant dierences b etween the mo del predictions and the data Most notably the calculated

velo city prole is quite symmetric while all the measured velo city proles are consistently

asymmetric At the force level the ma jor discrepancy is in the x comp onent There is an

apparent dominance of the right arm over the left arm and neither our dynamic mo del

nor the minimum torquechange criterion incorp orates this eect It is worth noting how

ever that this dominance app ears to b e indep endent of whether the sub ject is lefthanded

righthanded or ambidextrous Finallywehave not attempted to compare the predicted

results with the measurements for the other motions when the amplitude of motion in the

x direction is signicant b ecause of the variability of the force data across sub jects

Discussion

In this chapter the ob jectivewas to study and mo del human twoarm motions We

rep orted on exp eriments of human sub jects p erforming a planar manipulation task with

two arms and attempted to explain the observed b ehavior with a mo del based on the

minimum torquechange criterion

The observed tra jectories were approximately straight lines with b ellshap ed velo city

proles along the x and y directions and they were quite rep eatable The measured

edly asymmetric in the x direction and force distributions among the two arms were mark

symmetric in the y direction However no consistent pattern could b e observed among

dierent sub jects except in sagittal plane movements where the right arm app eared to b e

dominant regardless of whether the sub jects were left or righthanded

In the minimum torquechange mo del the ob jective function is the integral of the

vector of derivatives of the actuator forces Unlike other mo dels the minimum torque

change criterion predicts the internal forces in addition to the tra jectories of the system

and thus resolves indeterminacies in the task space joint space and actuator space This

is esp ecially imp ortant in the twoarm manipulation task where the internal forces can b e

seen as a measure of the interaction b etween the two hands The minimum torquechange

mo del seemed to reasonably predict the kinematic characteristics of the exp erimentally

observed motions and the total force histories However it was unable to account for the

observed dominance of the rightarmover the left arm in sagittal plane motions

While single jointmovements and single arm movements have b een studied extensively

this work see also is the rst quantitative study of human manipulation using two

arms The initial motivation for this study was to understand human motion planning

with the aim of improving control and planning for multiplearm rob otic systems The

exp erimental data was subsequently used to investigate if the metho d develop ed for motion

planning in rob otics can b e used to mo del human tra jectory generation

Chapter

Motion planning for systems with

discrete and continuous state

When we stated the motion planning problem we implicitl y assumed that the dynamic

description of the system do es not change during the task In many rob otic applications

this assumption do es not hold and the dynamic equations change as the system moves

Take for example a multingered hand holding an ob ject If the set of ngers which

are in contact with the ob ject do es not change the motion of the ob ject relative to the

wrist is describ ed by a set of ordinary dierential equations and a motion plan for

such a system can b e generated by traditional techniques Now consider regrasping of

the ob ject where another nger comes in contact or one of the ngers is withdrawn from

the ob ject The o ccurrence of any suchevents causes a change in the system of ordinary

dierential equations describing the state of the system and a change in the algebraic

constraints For systems of this typ e the state space can b e therefore partitioned into

regions so that the motion of the system in each of the regions is governed by a dierent

set of ordinary dierential equations These regions can b e viewed as separate discrete

states with the dierential equations within each discrete state describing the evolution of

the continuous state Successful completion of a task necessitates transitions b etween the

discrete states and a motion plan therefore consists of a sequence of discrete states as well

as a continuous tra jectory of the system within each discrete state In the example of a

multingered hand holding an ob ject the sequence of discrete states corresp onds to the

grasp gait forawalking machine dierent sequences represent dierent gaits

The systems describ ed ab ovefallinto the general class of hybrid systems These are

volve continuous dynamics discrete phenomena that maychange the control systems that in

b ehavior of the system as well as continuous or discrete control laws There are dierent

approaches to mo deling of hybrid systems The most general mo del was prop osed by

Branicky et al Suitable for mo deling the systems describ ed ab ove are also mo dels

of Bro ckett and of Kohn and Nero de Motion planning for a hybrid systems is

dicult and is sub ject of much research The hybrid systems framework of Branicky et al

isusedtoprove the existence of optimal motion plans Computational techniques for

nding optimal hybrid control based on dynamic programming are prop osed in but

they have prohibitive complexity and are thus not practical for solving complex motion

planning problems

This chapter describ es a metho d for generating continuous motion plans for a restricted

class of hybrid systems that are encountered in rob otics In rob otic tasks we require that

the state variables such as p ositions and velo cities and in some applications accelera

tions and forces must vary smo othly precise denition of smo othness dep ends on the

application and the mo del chosen to describ e a task We are therefore interested in gen

erating smo oth motion plans that allow transitions b etween discrete states Part of the

motion plan should b e a sequence of discrete states for example the walking or grasp

gait According to the underlying idea of this dissertation the motion planning problem

is formulated as an optimal control problem and weachieve smo othness by sp ecifying an

appropriate cost function We can also prescrib e constraints at the p oints where the system

switches b etween discrete states The resulting optimal control problem is very complex

so we simplify it by assuming that the sequence of discrete states is known We argue that

in many rob otic applications this sequence can b e obtained separatelyWe develop a tech

nique that uses the given sequence of discrete states to convert a problem with unknown

switching p oints to a problem in the standard form

We illustrate the metho d by computing smo oth motion plans for ne manipulation of

a circular ob ject with a twongered hand In this case the discrete states corresp ond to

dierent grasp congurations and the motion plan consists of a sequence of grasp congu

rations discrete states as well as a tra jectory evolution of the continuous state within

each grasp conguration Given a sequence of grasp congurations grasp gait we gener

ate a smo oth motion plan for moving the system through these congurations The motion

plan also sp ecies when to switchbetween dierent congurations

Problem formulation

Let the continuous state space X of the dynamical system with n states b e given by

p

D X

j

j

n

where D are pairwise disjoint connected subsets of IR Oneach subset D the system

j j

is describ ed with system equations

x F x u t

j

end algebraic constraints

G x u t

j

H x u t

j

n m

The vector x X IR is the continuous state of the system u IR F is a smo oth

j

vector eld and G and H are smo oth vector functions The sets D are called discrete

j j j

states and they partition the state space into regions so that on each region the evolution

of the system is governed by a dierent vector eld F and is sub ject to a dierent set of

j

algebraic constrain ts We require that the continuous state changes continuously but not

smo othly b etween the regions

Since the sets D are disjoint given a continuous state x there is a unique set D

j

j x

such that x D We can therefore rewrite as

j x

x f x u t

sub ject to

g x u t

hx u t

where f x u t F x u t g x u t G x u t and hx u t H x u t

x j x j x j D a 6 D7 D1 D4 A b c B C

D2 D5 D8

D3

Figure A schematic of a system with changing dynamic b ehavior

Figure schematically illustrates such a system The continuous state space is parti

tioned into eight discrete states regions The shaded areas in the state space are regions

which are not accessible for example where the rob ot would p enetrate an obstacle Three

sample tra jectories denoted by a band c are shown Given a starting p oint A and an

end p oint B itmay b e p ossible to go from A to B without changing the discrete state

system equations following the tra jectory a Alternatively the tra jectory b exhibits a

change from D to D and then a transition backtoD The optimal path b etween A

and C may follow a straightlinein D until it hits a b oundary travel along the b oundary

until a state transition to D o ccurs and then follow a straight line in D Since along a

the continuous state evolves within the same discrete state the tra jectory is smo oth On

the other hand b and c are only piecewise smo oth

Wenow dene the motion planning problem for the system Our premise is that

the task provides a wa ytoevaluate the p erformance of dierent motion plans where the

p erformance can b e formally describ ed with a cost functional We can therefore regard the

motion planning problem as an optimal control problem

Problem Given a system described by an initial state x and a nal state x

nd a set of inputs u that minimizes the cost functional

Z

T

Lx u tdt J

T

To each tra jectory x u t of that connects state x with x there corresp onds

a sequence of p oints T t t t T and a sequence of indices j j

N N

such that on the interval t t the tra jectory b elongs to the set D and at the time

i i j

i

t it switches from the region D to D Times t thus corresp ond to switches in the

i j j i

i i

discrete state of the system This implies that a solution of the optimal control problem

consists of four comp onents

a The number of switches N between discrete states

N

b The sequence fD g of discrete states regions that the system traverses as it

j

i i

moves along the optimal tra jectory

N

of times when the switches o ccur c The sequence ft g

i

i

d The tra jectories for the continuous state and the inputs on eachinterval t t

i i

It is dicult to nd all four comp onents of the optimal solution see also One

of the reasons is that in general the optimal solution dep ends on the sequence of discrete

states in very complicated way even if two sequences are almost identical tra jectories of

the continuous state can b e very dierent For this reason we can not use minimization

metho ds to nd the b est sequence and wemust p erform an exhaustive search of the space of

all sequences assuming that we can limit the length of the sequence To nd a sub optimal

solution we can solve the problem hierarchically we rst nd a sequence of discrete states

and then compute the tra jectory for the continuous state which is optimal for the chosen

sequence The sequence of discrete states can b e for example the shortest path in a

weighted graph that approximates the optimization problem Wedevelop a metho d for

computing the optimal continuous tra jectory for a given sequence of discrete states in the

next section

Metho d for computing switching times

In tasks such as grasping with a multingered hand or moving a legged mechanism on the

ground the sequence of discrete states is usually known a prioriFor the grasping task it

can b e obtained byinvestigating feasible grasp gaits In walking the gait is usually

computed in advance to avoid regions that are unsuitable for fo ot placement

Another instance where the sequence of discrete states is a priori known is planning op en

lo op tra jectories for transition b etween a free motion and p osition control to a constrained

motion and force control

When the sequence of discrete states is given the optimal control problem reduces

to nding the optimal switching times and the optimal tra jectories for the continuous state

This problem is still dicult to solve but weshow that it can b e reduced to a simpler

problem The main idea is to make the unknown switching times part of the state and

intro duce a new indep endentvariable with resp ect to which the switching times are xed

The resulting optimal control problem can then b e solved using available metho ds

N

Assume that the number N and the sequence of discrete states fD g through which

j

i i

the system evolves are known Without loss of generalitywe can assume that T t

and T t We can pro ceed in a similar way as in solving b oundary value

N

problems with unknown terminal time The rst step is to intro duce new state

variables x x corresp onding to the switching times t with

n nN i

x t

ni i

x

ni

We next intro duce a new indep endentvariable s The relation b etween s and t is linear

but the slop e of the curvechanges on eachinterval t t We establish piecewise linear

i i

corresp ondence b etween time t and the new indep endentvariable s so that at every chosen

xed p oint s where i N t equals t For convenience wesets iN

i i i

but any monotonically increasing sequence of N n umb ers on interval could b e used

Figure illustrates this idea As a result we obtain the following expressions

N x s s

n

N

N x x s

ni ni

i i

s i x ix

t

ni ni

N N

N x s

nN

N

s N x N

nN

N

t

tN+1

tN

......

t2

s t1

0 1/(N+1) 2/(N+1) . . . N/(N+1) 1

Figure A new indep endentvariable has xed values at the switching times t

i

With the new indep endentvariable the evolution equation on the interval t t that

i i

was given by

x F x u t

j

i

b ecomes



x N x x F x u s

ni ni j

i



where denotes the derivativeof with resp ect to the new indep endentvariable s and

F x u s F x u ts

If we denote byx the extended state vector

T

x x x x x

n n nN

i i

s we can dene on eachinterval

N N

Lx u s N x x Lx u ts

ni ni

Finallywe can rewrite the functional as

Z

Lx u sds J

and the task is to minimize J in the extended state space Points s at which the system

i

describ ed by the function F switches b etween the discrete states are known In the optimal

N

solutionx the last N comp onents will b e the optimal switching times ft g for the

i

i

original problem

Illustrative example

We study an example of two ngers with limited workspace rotating a circular ob ject in

ahorizontal plane In a similar example is used to study grasp gaits Our approach

is also in line with the conceptual framework prop osed in for motion planning for

dextrous manipulation For our task we require that exactly one nger is on the ob ject

during any nite time interval an example of suchtaskwould b e a driver turning a steering

wheel This eectively partitions the continuous state space into two discrete states each

representing the case when one nger is in contact with the ob ject and the other freely

moves in the plane There is also an intermediate third state when b oth ngers are in

contact with the ob ject through which the system passes instantaneously at each switch

We assume that the ob ject can rotate around a xed axis passing through the center of

the ob ject and that the ngers can exert arbitrary forces on the ob ject The motion of

each nger is restricted to a cone in the plane of the ob ject We can easily generalize

the example to the case of a multingered hand manipulating a planar ob ject or include

frictional constraints

Mathematical formulation

The p osition of the ob ject is given by its turning angle Figure The center of the

ob ject and thus the pivot p oint is at the origin of the global co ordinate system and the

radius of the ob ject is equal to RPositions of the two ngers in the plane are expressed

in p olar co ordinates and are r and r The dynamics of the ob ject is given by

I F R sin F R cos

x y

F R cos F R sin

y x

where I is the moment of inertia of the ob ject around the axis of rotation while F and F

are the forces expressed in the global co ordinate frame that the ngers and exert on

the ob ject y

θ2 (r2,θ2)

ϕ r2 x

R 1 r θ1

(r1,θ1)

Figure Two ngers rotating a circular ob ject in a plane

The dynamic equations of the two ngers can b e expressed in Cartesian co ordinates

For simplicitywe assume that the ngers b ehavelikepoint masses lo cated at the nger

tip The dynamics of the nger i is thus given by

m r cos r sin r cos r sin F u

i i i i i i i i i i i i ix ix

F u sin r cos m r sin r cos r

iy iy i i i i i i i i i i i i

where m is the eective mass of the nger iandu is the vector of driving forces

i i

for nger i By dening a state vector

T

x r r r r

the dynamic equations of the ob ject and the two ngers can b e transformed into the

statespace form The vectors u and u are the inputs to the system

We assume that the workspace of the two ngers is limited The workspace of each

nger is a cone of angle centered at the origin The axis of the cone for the rst nger

corresp onds to the halfline while the axis of the cone for the second nger is the

halfline The cones W and W representing the workspace of the ngers and

resp ectively are thus given by

W f j g

W f j g

For our task of rotating the ob ject we require that exactly one nger is on the ob ject

during any nite time interval It is easy to see that this requires partitioning of the state

space into three regions

D f x j W W r Rr Rg

D f x j W W r R r Rg

r R r Rg D f x j W W

Region D corresp onds to the case when the second nger is in contact with the ob ject

while the rst nger do es not touch the ob ject Region D describ es the opp osite situation

In region D b oth ngers are on the ob ject Because of the requirement that exactly one

nger is on the ob ject during any nite time interval the system can not stayin D This

basically reduces the state space to D D with the switchbetween the two corresp onding

to D

The dynamic equations are the same in all three regions Therefore f f f

where f is the system function obtained when the dynamic equations are rewritten in the

state space However in the region D the constraint r forcesr Similarlythe

requirement r implies F Analogous equations hold for D

Finallywehavetocho ose the cost functional for the optimal control problem The

choice dep ends on the desired prop erties of the optimal tra jectories For example contin

uous force proles can b e obtained by minimizing the rate of change of forces In this work

wechose to minimize a measure of the energy necessary to movethetwo ngers

Z Z

u u u u dt J Ldt

x y x y

At t wehave the initial conditions

r R r R

r r

and we assume that the system immediately passes into the region D We require the

ob ject to b e rotated through Both ngers are prescrib ed to end their motion on the

ob ject but we are not interested where The nal conditions are therefore

r R r R

r r

In order to use the metho d from Section wehave to x the numb er of transitions

between the two regions in the state space Assume that the system switches only once

we rst rotate the ob ject with the second nger and then complete the rotation of the

ob ject with the rst nger Wenow apply the technique outlined in Section Let t be

the time when the system switches from D to D We extend the state vector x with t

and imp ose the state equation

t

Next we dene a new indep endentvariable s with the equation

t s s

t

t s t s

The cost function L from Equation b ecomes

t L s

Lx u t

t L s

Similarlyiff f f is the system function for the indep endentvariable t the new

system functions b ecome

f t f

f t f

In eachregionwe also have to satisfy the following constraints

D r R D r R

r R r R

F F

Analogous expressions can b e obtained if the ngers change their roles more than once

To solve the resulting optimal control problem we can now use one of the metho ds

in Chapter As the rstorder metho d did not converge to a minimum we used the

tages direct minimization metho d that was develop ed in Section One of the advan

of this metho d is that the adjointvariables and the multipliers are not the unknowns

for the optimal control problem they are only up dated after the solution of the optimal

control problem has b een found Including state inputs adjointvariables and the Lagrange

multipliers there are unknown functions for a single switch The unknowns for the

variational problem are only the state and input variables which amounts to unknown

functions The variational problem that has to b e solved in each iteration is therefore

signicantly simpler than for the metho d from Section and the computation time

decreases

Simulation results

For the simulation wechose R and m m The task was to rotate the ob ject

for counterclo ckwise Weshow motion plans for dierentchoices of the workspace for

the two ngers

3 60 Object Object Finger 1 Finger 1 Finger 2 2 Finger 2 40

1 20 Angle Velocity 0 0

-20 -1 (a) -40 -2 0 0.25 0.5 0.75 1 0 0.25 0.5 0.75 1

Time Time

Figure a Angles and and b velo cities and for the workspace

Figure shows the tra jectories when the workspace is given by We sub

tracted from to compare it with the other two angles The new indep endentvariable s

is shown on the abscissa Therefore the switchbetween the two regions o ccurs at s

For the optimal value for the switching time is t s In the rst half

of the task the nger rotates the ob ject while the nger is not in contact with the

ob ject This can b e clearly seen from the velo city plots where but the nger

moves along the surface of the ob ject with lower sp eed At s t s the ob ject

is rotated for and the two ngers switch the roles For s the nger moves

freely in space and continues its motion until it reaches the limit of the workspace In the

meantime the nger rotates the ob ject to and also reaches the limit of its workspace

3 60 Object Object Finger 1 Finger 1 Finger 2 2 Finger 2 40

1 20 Angle Velocity 0 0

-20 -1 (a) -40 -2 0 0.25 0.5 0.75 1 0 0.25 0.5 0.75 1

Time Time

Figure a Angles and and b velo cities and for the workspace

Figure shows the optimal tra jectories for This is a limiting case since the

rotation can b e barely achieved with a single switch the maximal achievable rotation

is For s the nger is not in contact with the ob ject and it moves to the lower

edge of the workspace Meanwhile the nger rotates the ob ject for the entire allowable

range At s t s the nger comes into contact with the ob ject

The nger is at the lower edge of the workspace so that it can subsequently use the whole

range to complete the rotation of the ob ject The switchbetween the discrete states

D and D o ccurs at t s Also in this example the ngers end their motion at the

upp er edge of the workspace

For the tra jectories in Figure the workspace of the ngers was In this

case the rotation of the ob ject for can not b e achieved with a single switch the ngers

change their roles twice The switches o ccur at s and s which corresp ond

to t s and t s In this case the pattern observed in Figure is rep eated

twice while one nger rotates the ob ject the other nger moves towards the lower edge of

its workspace to have a wider range of motion once it comes into contact with the ob ject

During the rst third of the maneuver nger rotates the ob ject almost for the entire

range of the allowable workspace of Meanwhile nger moves to the lower edge of

the workspace and subsequently it rotates the ob ject for almost in the second third

of the task While nger is rotating the ob ject nger moves backtowards the middle

of its allowable workspace so that it can complete the rotation of the ob ject in the third

stage In the third stage nger stays at the upp er edge of its workspace

We also make the following observations

The velo cities of the con tact p oints on the ob ject and on the nger at the time 3 60 Object Object Finger 1 Finger 1 Finger 2 2 Finger 2 40

1 20 Angle Velocity 0 0

-20 -1 (a) -40 -2 0 0.33 0.66 0.99 0 0.33 0.66 0.99

Time Time

Figure a Angles and and b velo cities and for the workspace

of establishing or breaking the contact are equal so no impact o ccurs Hence the

continuous state is continuous across the switches b etween discrete states

The p ositions of the ngers on the ob ject at the switch are computed as part of

the motion plan In other words the sequence of discrete states is assumed to b e

known but the value of the continuous state at the switches is computed from the

optimization

The workspace constraints of the two ngers are resp ected We could further gener

alize the task by including frictional constraints

Discussion

The existing approaches to motion planning for systems with discrete and continuous state

concentrate either on planning the sequence of discrete states to achieve the task or on

planning and control of the system within each of the discrete states Several authors

address the problem of planning the task sequence for planar manipulation Erdmann

and Mason study manipulation sequences for orienting p olygonal ob jects in the plane

This work is extended in Similar is also the work by Goldb erg on xturing for

assembly although the manipulation sequence in this case consists of a single element

Planning of grasp gaits is studied by Leveroni Salisbury while Donald et al

describ e planar manipulation with multiple mobile rob ots These studies are based on

quasistatic analysis and it is not clear whether the results can b e used when the quasi

urther the emphasis is on the algorithmic issues of planning static assumption is violated F

the manipulation sequence The metho d describ ed in this chapter is an attempt to relax

the quasistatic assumption from the ab oveworks and replace it with dynamic analysis In

this way the problem of planning the manipulation sequence can b e addressed in the same

framework and at the same level as planning the op enlo op tra jectories for the control of

the dynamical system Although we had to assume that the sequence of the discrete states

is planned in advance our approach guarantees that the discrete sequence is compatible

with the dynamics of the system

We formulated the motion planning problem for systems with continuous and discrete

states as an optimal control problem This optimal control problem has two imp ortant

features

Dynamic equations describing the system change during the task It is dicult to

address suchchanges within the framework of optimal control Weshowed that the

optimal control problem can b e simplied if the sequence of discrete states for the

task is given

Partition of the state space into regions representing dierent discrete states can

b e usually describ ed with a set of equality and inequality constraints The motion

planning metho d must b e therefore capable of dealing with such constraints

We demonstrated the approach on the example of two ngers rotating a circular ob ject

in a plane In this case the continuous state space of the system is partitioned into

regions discrete states that corresp ond to dierent grasp congurations By cho osing the

cost function appropriately we can guarantee desired level of smo othness in the velo cities

accelerations and forces as the system switches b etween discrete states

The main limitation of the prop osed approach is that the sequence of discrete states

must b e known a prioriHowever the value of the continuous state at the switches b etween

discrete states is computed as part of the optimization which is imp ortant for example in

walking where this value corresp onds to placement of the legs on the ground

Chapter

Task space and the sp ecial

Euclidean group SE


In the next twochapters we turn our attention to kinematic motion planning We concen

trate on planning in the task space but similar framework could b e used to study planning

in the joint space When the tra jectories are planned in the task space we can take ad

vantage of the sp ecial structure of this space In this chapter we establish the dierential

geometric framework for study of the task space and investigate some prop erties of the

space that are imp ortant for kinematic analysis and kinematic motion planning

Position and orientation of an endeector that moves in IR can b e represented with

the rigidb o dy displacementbetween an inertial reference frame and a frame attached to

the endeector The task space can b e therefore represented with the set of rigid b o dy

displacements in IR This set forms a group known as the sp ecial Euclidean group in

three dimensions SE The structure of SE is very dierent from the structure of a

Euclidean space IR but lo cally there exists a bijection b etween the two This implies that

the group SE is a manifold and therefore a Lie group Because of the nonEuclidean

structure of the group SE we study its prop erties within the framework of dierential

geometry which is a natural generalization of dierential calculus from the Euclidean

spaces

In motion planning and other applications we need to measure distances on SE

The notion of distance on a manifold is provided bycho osing a Riemannian metric Once

w echo ose a metric the manifold b ecomes rigid since the distances b etween the p oints

get xed A weaker structure on the manifold is obtained byintro ducing an ane connec

tion With an ane connection we can dierentiate vector elds and dene the covariant

derivative a generalization of dierentiation from Euclidean space The ane connection

is used to formally dene acceleration and higher derivatives of the velo cityFor motion

planning metrics and connections are crucial since they are used to dene cost functionals

for measuring smo othness of kinematic tra jectories

The tangent space to SE at the identity denoted by se has the structure of a Lie

algebra The Lie algebra se is isomorphic to the set of twists which is used for velo city

analysis in kinematics An inner pro duct on se can b e extended to a

Riemannian metric on SE It is well known that there is no inner pro duct on the space

of twists that is invariant under the change of the inertial and b o dyxed reference frames

that are used to describ e motion of a rigid b o dy But on se there exist two

quadratic forms the Killing form and the Klein form whichareinvariant under changes of

the reference frames However neither form denes an inner pro duct the Killing

form is degenerate while the Klein form is indenite

After a brief overview of some basic notions from dierential geometryweinvestigate

metrics and connections on SE that are imp ortant for kinematic analysis Chasless

theorem guarantees the existence of a screw motion between anytwopoints on SE

Given a metric on SE we can also nd a shortest distance curvebetween anytwo

p oints with resp ect to this metric Such curves are called geodesics and can b e considered

a generalization of straight lines from Euclidean geometry to Riemannian manifolds A

natural question is whether there exists a metric for which geo desics are screw motions

Weshow that no such Riemannian metric exists We also identify a twoparameter family

of semiRiemannian metrics which consists precisely of the metrics having screw motions

for geo desics These metrics can b e obtained from the biinvariant forms on se

In the next section we study connections that lead to physically meaningful acceler

ation We show that there is a unique symmetric connection that yields the acceleration

that is used for kinematic analysis We also identify a family of Riemannian metrics that

are compatible with this connection Any metric in this family is a pro duct of a biinvariant

R metric on SO and a Euclidean metric on I

Kinematics Lie groups and dierential geometry

Consider a rigid b o dy moving in free space Assume any inertial reference frame fFg xed



in space and a frame fMg xed to the b o dy at p oint O as shown in Figure Ateach

instance the conguration p osition and orientation of the rigid b o dy can b e describ ed by

a homogeneous transformation matrix corresp onding to the displacement from frame fFg

to frame fMg The set of all such matrices is called SE the sp ecial Euclidean group of

rigid b o dy transformations in threedimensions

R d

 T

SE j R IR d IR R R I detR

It is easy to show that SE is a group for the standard matrix multiplication and

that it is a manifold It is therefore a Lie group

On any Lie group the tangent space at the group identity has the structure of a Lie

algebra The Lie algebra of SE denoted by se is given by

v

 T

se j IR v IR

A skewsymmetric matrix can b e uniquely identied with a vector IR so that

for an arbitrary vector x IR x x where is the vector cross pro duct op eration

in IR Each element S se can b e thus identied with a vector pair f vg

A screw motion is a rigid b o dy motion in which the rigid b o dy rotates ab out an axis while concurrently

translating along that axis It will b e formally dened later in the chapter z' {M} z {F} FINAL POSITION y' p INITIAL P POSITION p O' d y O

x'

x

Figure The inertial xed frame and the moving frame attached to the rigid b o dy

Given a curve Ata a SE an element S t of the Lie algebra se can b e

asso ciated to the tangentvector At at an arbitrary p oint t by

T T

d R R R



S t A tAt

A curveonSE physically represents a motion of the rigid b o dyIff tvtg is the

vector pair corresp onding to S t then physically corresp onds to the angular velo city



of the frame fMgboth of the rigid b o dy while v is the linear velo city of the origin O

expressed in the frame fMg In kinematics elements of this form are called twists

and se thus corresp onds to the space of twists It is easy to check that the twist S t

computed from Equation do es not dep end on the choice of the inertial frame fFg

For this reason S t is called the left invariant representation of the tangentvector A

Alternatively the tangentvector A can b e identied with a rightinvarianttwist invariant

with resp ect to the choice of the b o dyxed frame fMg

Since se is a vector space any element can b e expressed as a vector of comp o

nents corresp onding to a chosen basis The standard basis for se is

L L L

L L L

The twists L L and L represent instantaneous rotations ab out and L L and L in

stantaneous translations along the Cartesian axes x y and z resp ectively The comp onents

vg of a twist S se in this basis are given precisely by the velo cityvector pair f

The Lie bracket of two elements S S se is dened by

S S S S S S

It can easily b e veried that if f v g and f v g are vector pairs corresp onding to the

twists S and S the vector pair f vg corresp onding to their Lie bracket S S isgiven

by

f vg f v v g

In kinematics this pro duct op eration is called the motor pro duct of the twotwists

The Lie bracket of two elements of a Lie algebra is an element of the Lie algebra

k

and can b e expressed as a linear combination of the basis vectors The co ecients C

ij

corresp onding to the Lie brackets of the basis vectors are called structureconstants of the

Lie algebra

X

k

C L L L

k i j

ij

k

The expressions for se can b e directly computed from Equation and are

listed in App endix E

Left invariantvector elds

A dierentiable vector eld is a smo oth assignment of a tangentvector to each element

of the manifold Ateach p oint a vector eld denes a unique integral curve to which

it is tangent Formallyavector eld X is a derivation op erator which given a

dierentiable function f returns its derivative another function along the integral curves

of X In other words if tisa curve tangenttoa vector eld X at p oint p t then

df t

X f j

p

dt

t

We will b e particularly interested in left invariant vector elds on SE From any

twist S se we can generate a dierentiable vector eld S by assigning to all A

SE a vector S A in the tangent space at that p ointby the formula

S AAS

The vector eld generated by Equation is called a left invariantvector eld and we

use the notation S to indicate that the vector eld was obtained by left translating the Lie

algebra element S Rightinvariantvector elds can b e dened analogously In general a

vector eld need not b e left or rightinvariant By construction the space of left invariant

L are its vector elds is isomorphic to the Lie algebra se and vector elds L L

basis Wealsohave see

X

k

d

L L L L L C

k i j i j

ij

k

Since the vectors L L L are a basis for the Lie algebra se the vectors

L A L A form a basis of the tangent space at anypoint A SE Therefore

anyvector eld X can b e expressed as

X

i

X X L

i

i

i

where the co ecients X are realvalued functions If the co ecients are constant then X

is left invariant Equation shows that

X X X

i i i

S AL S L A S L A AS A

i i i

i i i

We conclude that if the velo city of the rigid b o dy A is expressed in the basis L L

the comp onents with resp ect to this basis are equal to the comp onents of the instantaneous

twist

T

S S S

T

v S S S

We will therefore refer to the elements L L of the basis for the left invariantvector

elds as basis twists Equation also implies that is the vector corresp onding to the

T T

skewsymmetric matrix R R while v R d

Exp onential map and lo cal co ordinates

dA

is the vector eld A curve At on SE describ es a motion of the rigid b o dyIfV

dt

tangentto At the vector pair f vg asso ciated with V corresp onds to the instantaneous

twist screw axis for the motion In general the twist f vg changes with time Motions

for which the twist f vg is constant are known in kinematics as screw motions In this

case the twist f vg is called the screw axis of the motion If the vector pair f vg is

interpreted as Pluc ker co ordinates of a line in space it is not dicult to see that the screw

motion physically corresp onds to rotation around this line with a constant angular velo city

and concurrent translation along the line with a constant translational velo city

Let the twist S se b e represented byavector pair f vg and let At b e a screw

motion with the screw axis f vg such that A I We dene the exponential map

exp se SE by

exptS At

Using Equation we can show that the exp onential map agrees with the usual exp o



nentiation of the matrices in IR



k k

X

t S

exptS

k

k

where S denotes the matrix representation of the twist S The sum of this series can b e

computed explicitly and the resulting expression when restricted to SO is known as

Ro drigues formula The formula for the sum in SE is derived in

Given a twist T t the motion of the rigid b o dy is given by

dAt

AtT t

dt

Since for all t T t b elongs to the Lie algebra se it can b e expressed as a linear

combination with varying co ecients of the basis vectors According to Wei and Norman

the solution of this dierential equation can b e written as the pro duct of exp onentials

in the form

Y

i

exp tL At

i

i

i i

where t are analytic functions The co ecients only dep end on A and can b e

therefore taken as a set of lo cal co ordinates This representation of SE is valid in some

neighb orho o d of the identity element and is not global The co ordinates obtained from

are called the canonical coordinates of the second kind

Although the order of taking the pro duct in is arbitrary once chosen wemust

adhere to it to get consistentvalues for the co ordinates since in general

i j j i

exp L exp L exp L exp L

i j j i

Wecho ose the following ordering

A L exp L exp L t exp

An alternativeway of dening a parameterization for SE is to use the so called

canonical co ordinates of the rst kind

A exp L L L L L L

Also this parameterization is only valid in some neighb orho o d of the identity

Riemannian metrics on Lie groups

If a smo othly varying p ositive denite bilinear symmetric form is dened on

the tangent space at each p oint on the manifold suchformiscalledaRiemannian metric

and the manifold is Riemannian If the form is nondegenerate but indenite the

iemannian On a ndimensional manifold the metric is lo cally metric is called semiR



characterized byan n matrix of C functions g X X where X are basis vector

ij i j i

elds If the basis vector elds can b e dened globally then the matrix g completely

ij

denes the metric

On SE on any Lie group an inner pro duct on the Lie algebra can b e extended

to a Riemannian metric over the manifold using left or right translation To see this

consider the inner pro duct of two elements S S se dened by

T

S S j s Ws

I

where s and s are the vectors of comp onents of S and S with resp ect to some

basis and W is a p ositive denite matrix If V and V are tangen tvectors at an arbitrary

group element A SE the inner pro duct V V j in the tangent space T SE

A

A

can b e dened by

 

V V j A V A V

A

I

The metric obtained in suchaway is said to b e left invariant since left translation by

any element A is an isometry



The basis vector elds need not b e the co ordinate basis we only require that they are smo oth

Ane connection and covariantderivative

Motion of a rigid b o dy can b e represented bya curveonSE The velo cityatan

arbitrary p oint is the tangentvector to the curve at that p oint In order to obtain the

acceleration or to engage in a dynamic analysis we need to b e able to dierentiate a vector

eld along the curve Ateach p oint A SE the value of a vector eld b elongs to the

tangent space T SE and to dierentiate a vector eld along a curve wemust b e able to

A

subtract vectors from tangent spaces at dierent p oints on the curve But tangent spaces

at dierentpoints are not related Wethus havetospecifyhowtotransportavector along

the curve from one tangent space to another This pro cess is called paral lel transport and

is formalized with the ane connection Given any curve At a parameter value

t and a vector V in T SE the tangent space at p oint At the ane connection

At

 

assigns to each other parameter value t avector V T SE By denition V is the

At



parallel transp ort of V along the curve At Vectors V and V are also said to b e paral lel

along At

A derivativeofavector eld along a curve At is dened through the parallel transp ort

Let X be a vector eld dened along At and let X t stand for X At Denote by

t

X t the parallel transp ort of the vector X ttothepoint At The covariant derivative

of X along At is

t

X t X t DX

lim

tt

dt t

t

By taking covariantderivatives along integral curves of a vector eld Y we obtain a

covariant derivative of the vector eld X with resp ect to the vector eld Y This derivative

is also denoted by r X

Y

DX

r X j

Y

A

dt

t

DX

where is taken along the integral curveofY passing through A at t t

dt

The notions of the covariantderivative and parallelism are equivalent In this section

we dened the covariant derivative through parallel transp ort However given a covariant

derivative wecanalways dene that a eld X is parallel along a curve Atif

X r

dA

dt

The covariant derivativeofavector eld is another vector eld so it can b e expressed

k

as a linear combination of the basis vector elds The co ecients of the covariant

ji

derivativeofabasisvector eld along another basis vector eld

X

k

r L L

j k

ji

L

i

k

are called Christoel symbols Note the reversed order of the indices i and j

The velo city V t of the rigid b o dy describing the motion Atisgiven by the tangent

vector eld along the curve

dAt

V t

dt

In the literature dierent denitions for the Christoel symb ols can b e found Some texts eg

reserve the term for the case of the co ordinate basis vectors We follow the more general denition from

in which the basis vectors can b e arbitrary

The acceleration At is the covariant derivative of the velo city along the curve

dA D

r V A

V

dt dt

Note that the acceleration dep ends on the choice of the connection We can also dene

jerk J as the covariant derivative of the acceleration

D

J At r r V

V V

dt

Given a Riemannian manifold there exists a unique connection which is compatible

with the metric

X YZ r Y Z Y r Z

X X

and symmetric

r Y r X X Y

X Y

This connection is called the LeviCivita or Riemannian connection It can b e shown

that the compatibility condition is equivalenttosaying that the parallel transp ort

preserves the inner pro duct In other words if At is a curveandX and Y are twovector

elds obtained by parallel transp orting twovectors X and Y from T SE along At

A

X Y j const then XY j

t A A

Geo desics

Given a Riemannian metric on SE we can dene the length LA of a smo oth

curve A a b SE by

Z

b

dA dA



dt LA

dt dt

a

Among all the curves connecting twopoints we are usually interested in the curveof

minimal length It is not dicult to see that a curve of minimal length also minimizes

the energy functional

Z

b

dA dA

E A dt

dt dt

a

If a curve minimizes a functional it must also b e a critical p oint Critical p oints of the

energy functional E satisfy the following equation

dA

r

dA

dt

dt

where r is the Riemannian connection and are called geodesicsFromwhatwe said ab out

the covariant derivative and parallel transp ort it follows that a geo desic is a curve At

dA

for which the tangentvector eld is parallel from a value at a p ointwe can obtain

dt

its value at any other p ointby simply parallel transp orting it along the curve At On

dA

is the acceleration of the other hand According to Equation the expression r

dA

dt

dt

motion describ ed by At Motion along a geo desic therefore pro duces zero acceleration

Metrics and screw motions

One of the fundamental results in rigid b o dy kinematics was proved by Chasles at the

b eginning of the th century

Theorem Chasles Any rigid body displacement can berealizedbya rotation about

an axis combinedwithatranslation paral lel to that axis

Note that a displacementmust b e understo o d as an elementofSE while a motion

is a curveonSE If the rotation from the Chasless theorem is p erformed at constant

angular velo city and the translation at constant translational velo city the motion leading

to the displacement clearly b ecomes a screw motion Chasless theorem therefore says that

any rigid b o dy displacement can b e realized with a screw motion

Another family of curves of particular interest on SE are the oneparameter sub

groups A curve At is a oneparameter subgroup if At t At At The

oneparameter subgroups on SE are given by

At exp tS

where S is an elementofse From our discussion in Section it is clear that the

one parameter subgroups are exactly the screw motions which start at the identity In

the Chasless theorem wecanobviously assume that the b o dy xed frame fMg is initially

aligned with the inertial frame fFg Therefore in the language of dierential geometry

the theorem can b e restated as follows

Theorem Chasles restated For every element in SE there is a oneparameter

subgroup screw motion through the identity to which that element belongs

Except for the identity the screw axis for every element is unique but there are in

nitely many screw motions along that axis whichcontain the element eachcharacterized

by the numb er of rotations along the screw axis The following corollary immediately

follows from the theorem

Corollary If A and A are two distinct elements of SE then

Thereexistsaoneparameter subgroup texptS which when left translated

L L

contains A by A

A t A exp tS A A A exp S

L L L L

There exists a oneparameter subgroup t exp tS which when right trans

R R

latedby A contains A

A t exptS A A A exp S A

R R R R

For each S inwecan nd the corresponding S in by

L R



S A S A

R L

and in this case

A tA t

L R

t resp ectively follows from Theorem if we left right translate

Proof: Statemen

 

the oneparameter subgroup to which A A A A b elongs To see note that

 



k k k k

X X

t A S A t S

L

L

A A exp tS A

L

k k

k k

Park and Bro ckett prop osed a left invariant Riemannian metric on SE given

by

I

W

I

where and are p ositive scalars

Park derived the geo desics for the metric and showed that they are pro ducts

of the geo desics for the biinvariantmetriconSO and geo desics in the Euclidean space

IR Geo desics for the biinvariant metric on SO are the restrictions of the screw motions

to SO while straight lines parameterized prop ortionally to the line length are the

geo desics for the Euclidean space IR A geo desic b etween two elements

R d R d

and



thus physically corresp onds to a translation of the origin O of the b o dy xed frame fMg

with a constant translational velo city along the line connecting the p oin ts describ ed with

the p osition vectors d and d and concurrent rotation of the frame fMg with constant



angular velo city ab out an axis passing through the origin O of the frame fMg which



translates together with the p oint O It is clear that such motion is in general not a screw

motion since the axis around which the b o dy rotates is not xed in space However if the

axis of rotation is collinear with the line b etween d and d along which the b o dy translates

the rotational axis do es not change as the b o dy moves and the geo desic is therefore a screw

motion It follows that a screw motion is a geo desic if and only if it is obtained by left

translation of a oneparameter subgroup for which the screw axis passes through the origin

O of the frame fFg

Screw motions as geo desics

Given that anytwo elements of SE can b e connected with a screw motion and given

that there exists a left invariant metric whose geo desics include certain screw motions it

is natural to ask whether there are Riemannian metrics for whichevery geo desic is a screw

motion

Before we pro ceed we turn our attention back to Corollary The corollary says that

any screw motion can b e obtained in twoways either by a left or by a right translation of

a in general dierent oneparameter subgroup Now supp ose that the screw motions are

geo desics Corollary implies that a left or a right translation of a geo desic pro duces

another geo desic We might therefore wrongly conclude that any metric for which the screw

motions are geo desics must b e invariant under left and right translations and therefore bi

invariant Such reasoning is false since a map which preserves geo desics do es not necessarily

preserve the metric is not an isometry This is clear if we consider ane transformations

in Euclidean space They map lines into lines but in general they do not preserve lengths

of vectors Therefore we can not limit our search only to the left or rightinvariant metrics

Wenowderive the family of metrics whichhave screw motions for geo desics As wesaw

in Section the twist asso ciated with a screw motion t is constant The tangent

d

vector eld V is therefore a left invariantvector eld and it has constant comp onents

dt

i

L If solves Equation we with resp ect to the chosen basis vector elds V V

i

have

j

X X X

dV

i j i j

r V L V V r L V V r L

V j j j

L L

i i

dt

j ij ij

The ab ove equation is satised for any screw motion arbitrary choice of the comp onents

i

V if and only if

r L r L

j i

L L

i j

Since r is a metrical connection it is symmetric Equation

r L r L L L

j i i j

L L

i j

It immediately follows that

L L L r

i j j

L

i

Further r must b e compatible with the metric Equation so wehave

L L L r L L L r L

k i j i j i j

L L

k k

Letting g L L the last equation implies

ij i j

L g L L L L L L

k ij k i j i k j

By expressing the Lie brackets from Equation we nally obtain

X

l l

g g C C L g

li lj k ij

kj ki

l

k

Note that the co ecients C are constantover the manifold Equation The ab ove

ij

derivation is summarized in the following prop osition

Prop osition Screw motions satisfy the geodesic equation for a Riemannian

metric given by the matrix of coecients G g if and only if the coecients g satisfy

ij ij

Equation

The metric co ecients g are symmetric by denition Since SE is a sixdimensional

ij

manifold there are indep endent co ecients fg j i j gFurther there are

ij

basis vector elds hence Equation expands to a total of equations Eachvector

eld represents a derivation implying that these are partial dierential equations The

complete set of equations is given in App endix C

We need the following lemma to derive the solution for the system of equations given

by

Lemma Given a set of partial dierential equations

X f g

x

Y f g

y

Z f g

z

where X Y and Z arevector elds such that Z X Y f is twicedierentiable and

g g and g aredierentiable real valued functions the solution exists only if

x y z

X g Y g g

y x z

X on Equation Y on Equation and subtracting the two

Proof: By applying

resulting equations weget

XYf YXf X g Y g

y x

But the lefthand side is by denition X Y f whichisby assumption equal to Z f

Equation then follows from Equation

We next state the rst key theorem of this chapter

Theorem A matrix of coecients G g satises the system of partial dierential

ij

equations if and only if it has the form

I I

 

G

I

 

where and areconstants

o nd the metric co ecients we start with the following subset of C

Proof: T

L g L g g L g g

First observe that L L L see App endix E By application of Lemma the

following equation must hold

L g g

But from C wehave

g L g

Therefore Equation b ecomes

g g

Obviously this implies that g We next observe that g implies L g

i

i From the system C we obtain

g g g g

g g g g

From these equations and C we further obtain

g g g g

g g g g

g g g

g g g

Next observation is that L g i This together with Equations and

i

implies

g g g

where is an arbitrary constant Similarlywe obtain

g g g

for an arbitrary constant In this waywehave obtained all indep endentvalues of G

The reader can easily check that the system of equations C is satised by the ab ove

values

Corollary ThereisnoRiemannian metric whose geodesics arescrew motions

heck that a matrix of the form

Proof: It is easy to c

I I

 

G

I

 

has two distinct real eigenvalues

q q

whichbothhavemultiplicityFor anychoice of and the pro duct of the eigenvalues

is Therefore G is not p ositive denite as required for a Riemannian

metric

Invariance of the family of semiRiemannian metrics

Metrics of the form form a twoparameter family of semiRiemannian metrics and

can b e studied in a similar way as Riemannian metrics In particular we can investigate

their invariance prop erties By denition a metric is left invariantifforany A B SE

and for anyvector elds X and Y

XB Y B j AXB AY B j

B AB

t if and it is rightinvarian

XB Y B j XB A Y B Aj

B BA

Lemma If S and S are two elements of se and a metric of the form is

denedon SE then for any A SE

S S j Ad S Ad S j

A A

I I

The map Ad se se is called the adjoint map and if S is represented by a matrix



the map is dened byAd S AS A

A

Let S f v g and S f v g By a straightforward algebraic calculation it

Proof:

can b e shown that for S f vg se and A SE where

R d

A

the value of Ad S isgiven byAd S fRRv R dg where is the usual

A A

vector cross pro duct Therefore wehave

Ad S Ad S j

A A

I

fR Rv R dg fR Rv R dg j

I

T T T

R R R Rv R d R Rv R d

T T T T T

R R d v v R R d

T T T

v v f v g f v g j S S j

I I

Prop osition Any left invariant metric G that satises Equation is biinvariant

both left and right invariant

ehave to prove that G is rightinvariant Taketwovector elds X and Y Since

Proof: W

the metric G is left invariant wehave

XB A Y B Aj

BA

 

BA X B A BA Y B A

I

   

A B X B A A B Y B A

I

By Equation

   

B A A B Y B A A B X

I

 

B X B B Y B

I

But b ecause of the left invariance of G the last expression is

 

B X B B Y B XB Y B j

B

I

as required

Corollary Any metric G of the form is biinvariant

vious that a metric G of the form is left invariant since it is constant

Proof: It is ob

for the basis of the left invariantvector elds L By Lemma and Prop osition G is

i

biinvariant

Geo desics of the family of semiRiemannian metrics

Analogous to the Riemannian case we could dene the length of a curve Atbetween two

p oints At and At onSE by

Z

t



dA dA



LA t t dt

dt dt

t

But G is not p ositive denite so the length of a curvewould b e in general a complex

numb er Therefore it is more useful to dene the measure of the energy of a curve

Z

t



dA dA

dt E A t t

dt dt

t

Since G is not p ositive denite the energy of a curve can b e in general negative There

are also nontrivial curves that is curves that are not identically equal to a p oint which

have zero energy

Two sp ecial cases of metric are of particular interest With and we

obtain the metric

I

 

G

I

 

This metric taken as a quadratic form on se is known as the Klein form The eigenval

ues for the metric are f g and the form is therefore nondegenerate For

a screw motion At A exp tS where S f vg se the energy of the segment

T

y E A v If the quantity t is given b

T

v

h

j j

is called the pitch of the screw motion The pitch measures the amount of translation

along the screw axis during the screw motion Zero energy screw motions therefore either

have zero pitch the motion is pure rotation or innite pitch the motion is pure

translation Screw motions with p ositive energy are those with p ositivepitch Tra jectories

for such motions corresp ond to righthanded helices and the motions are thus called right

handed screw motions Analogously screw motions with negative energy are the left

handed screw motions Since pure rotations and pure translations are zeroenergy motions

it is always p ossible to nd a zero energy curvebetween two arbitrary p oints by breaking

the motion into a segment consisting of a pure rotation followed by a segment of a pure

translation

By letting and we get the semidenite metric

I

 

G

 

This metric as a form on se is called the Killing form Its eigenvalues are f g

T

hence it is degenerate The energy of a screw motion with S f vg is equal to

so it is always nonnegative Pure translations are zeroenergy motions while any motion

involving rotation has p ositive energy

In the general case and the energy of a unit screw motion At

T

A exp tS where S f vg and t is v Pure translations

thus have zero energyFor a general screw motion the energy of the segment

t is j j h The sign of the energy of a general motion therefore dep ends on

and

Ane connections on SE

There is no natural choice of a metric on SE In the previous section wechose a

particular family of curves and found the metric G for which these curves were geo desics

Through the metric connection a metric also provides a natural way to dierentiate vector

elds

In this section our primary ob jective is to nd a connection which pro duces an acceler

ation vectorthatisphysically meaningful We therefore start byintro ducing a connection

ariantderivative and obtain the acceleration on SE whichallows us to compute the cov

By requiring that the acceleration computed with the covariant derivative agrees with the

acceleration as computed in kinematics we obtain a family of p ossible ane connections

We then show that in this family there is a unique symmetric connection Finallywede

termine the class of Riemannian metrics which are compatible with this symmetric ane

connection

Kinematic connection

As discussed in Section the Lie algebra se represents the space of twists Wesaw

that the basis of left invariantvector elds L Equation provides a natural framework

i

for studying motion on SE The comp onents of tangentvector elds with resp ect to

this basis corresp ond to the comp onents of the instantaneous twist asso ciated with the

motion expressed in the b o dyxed co ordinate frame To obtain the acceleration wehave

to compute the covariant derivative of the velo city the tangentvector eld along the

curve describing the motion

Wenow turn our attention to the acceleration vector Let At b e a curve describing

motion of a rigid b o dy Let S t f vg represent the instantaneous twist of the rigid

body expressed in the moving frame fMg xed to the rigid b o dy More precisely

represents the angular velo city of the rigid b o dy while v is the linear velo city of the origin



O of the b o dy xed reference frame fMg see Figure Wewould like to dierentiate



the velo cityofthepoint O to obtain the acceleration The acceleration can b e represented

byavector pair f ag where is the angular acceleration of the rigid b o dy and a is the



acceleration of the p oint O b oth expressed in the frame fMg This acceleration vector

pair can b e shown to b e equal to

f ag f v g f v g

The rst term in this equation is simply the derivative of the comp onents of the angular



and linear velo city of the p oint O This term is also called the spatial acceleration

However angular and linear velo cities were expressed in the b o dy xed frame fMg which

rotates as the b o dy moves Wemust therefore add a con vective term to describ e the

contribution of this rotation to the acceleration of the rigid b o dy This term is an artifact

of expressing the velo cities in a frame that rotates

On the other hand geometrically the acceleration of the rigid b o dy Aisgiven by the

dA

along At covariant derivative of the velo cityvector eld V

dt

A r V

V

It is imp ortant to see the dierence b etween the twist S t used to obtain Equation

and the velo cityvector eld V tV At the twist S t b elongs to se while V t

b elongs to the tangent space T SE

At

i

L and let f vg b e the corresp onding vector of comp onents According Let V V

i

to Equation the ane connection that pro duces physically meaningful acceleration

must satisfy

r V f v g f v g

V

V can b e rewritten as In comp onents r

V

k

X X X

dV

i j k

r V V V L L

V k k

ji

dt

ij 

k k

k

are the Christoel symb ols which dene the ane connection for the basis where

ji

L Expressions and will b e the same if the rst and the second term in

i

Equation corresp ond to the rst and the second term in Equation resp ectively

Obviously the rst terms are the same regardless of the choice of the ane connection

i j

However b ecause of the symmetry the co ecient for the pro duct V V ij

k k k

is for i j the co ecientis Therefore from Equation we conclude

ji ij ii

that

k k k

i j a

ij ji ij

k

where a are constan ts that can b e directly obtained from Equation The only

ij

k

nonzero values a are

ij

a a a

a a a

k

The system do es not contain enough equations to solve for if i j for i j

ij

k

wehave Therefore there is a whole family of ane connections on SE that

ij

pro duce a physically meaningful acceleration

To further restrict the choice of the connection we might ask if any of the connections

that satisfy Equation can b e a Riemannian connection Since every Riemannian

connection is symmetric we require that in addition to Equation D the connection also

satises Equation

r Y r X X Y

X Y

It immediately follows that for the basis L the symmetry of the connection implies

i

k k k

C

ij ij ji

k

Equations and together uniquely determine the Christoel symb ols and

ji

therefore the connection We call this connection the kinematic connection The nonzero

Christoel symb ols for the kinematic connection are

Metrics compatible with the kinematic connection

As seen in Section a connection is Riemannian if and only if it is symmetric and

compatible with the metric Since we explicitly required that the kinematic connection

b e symmetric we can try to nd a metric which is compatible with the connection In

general such metric may not exist

If a metric is compatible with the connection then

ZXY r X Y X r Y

Z Z

where X Y and Z are arbitrary vector elds By substituting the basis vector elds L

i

L and L for X Y and Z the compatibility condition b ecomes

j k

X

l l

L g g g

k ij lj li

ik jk

l

k

where the Christoel symbols were computed ab ove Equation generates a

ji

system of partial dierential equations for metric co ecients fg j i j g

ij

j

Note that for k soL g The system of equations obtained from

k ij

ik

Equation for k is given in App endix D The rst step in nding the solution of

this system of equations is represented by the following lemma

Lemma If the coecients of a Riemannian metric G satisfy Equation the

metric has the form

I

G

G

p

where is a constant and G is an arbitrary positive denite symmetric matrix that

p

smoothly varies from point to point

e use Lemma again Take the following subset of equations of the system

Proof: W

D

L g L g g L g g

According to Lemma the following equality holds

L g g

g g whichgives g Substi By substituting for L g from D we obtain

tuting in the system D we next obtain

g g g g g g

It is easy to see that these equations imply L g i which together with

i

Equation results in

g g g

where is a constant Therefore the upp erleft blo ck in the matrix G is of the form

I whereI is the identity matrix



By taking equations

L g L g L g g g g g

and again using Lemma weget g By substituting this in the system D it is

easy to see that all the entries in the upp erright blo ck of the matrix G and due to

the symmetry of G also in the lowerleft block are equal to The matrix G must

b e p ositive denite symmetric therefore also G must b e p ositive denite symmetric

p

Prop osition A left invariant metric is compatible with the kinematic connection if

and only if the matrix of metric coecients G g is of the form

ij

I

G

I

where and are arbitrary constants

If a metric is left invariant then the matrix G is constant But if g const then

Proof: ij

L g It is then easy to check that the system of equations D implies the form

i ij

of G in

Note that the metric is exactly the same as the metric that weinvestigated

earlier

To determine whether there are other solutions of the system D it helps to investi

gate what is common to metrics whichhave the same metrical connection The geometric

entity determined from the connection are geo desics Equation Two metrics with

the same connection thus have the same geo desics By reasoning similar to that which

led to the kinematic connection we can prove that also the converse is true The family

of geo desics uniquely determines the symmetric connection To nd other solutions of the

system D we therefore try to nd metrics whichhave the same geo desics as metric

Alternatively this pro cess can b e viewed as the study of dieomorphisms of SE

which map geo desics to geo desics such maps are called ane maps

Wesaw earlier that a geo desic for the metric b etween twopoints A and A on

SE is a pro duct of a geo desic on SO equipp ed with a biinvariant metric a constant

velo city rotation around an appropriate axis and a geo desic on Euclidean space IR a

straigh t line But on IR straight lines are geo desics for an arbitrary inner pro duct dened

by a p ositive denite constant matrix Therefore any pro duct metric on SO IR with

the biinvariant metric on SO and an inner pro duct metric on IR has the same geo desics

as the metric It can b e shown see App endix A that for the basis L such metric

i

has the form

I

GR d

T

R WR

where W is a constant p ositive denite symmetric matrix which denes the inner pro duct

on IR We state this as a lemma

Lemma If a metric G has the form then it is a solution of the system of

equations D

Note that the form of G in Lemma is consistent with that in Lemma It is also

obvious that the metric can b e obtained as a sp ecial case of by substituting

W I

Lemma identies a family of Riemannian metrics that are compatible with the

kinematic connection Wewould liketoknow whether there are any other metrics which

are compatible with the kinematic connection Before we answer this question we prove

the following lemma

Lemma Two metrics on SE which have the same Riemannian connection and

areequal at a point areequal everywhere

Let G and G be the two metrics and let A b e the p oint where they are equal

Proof:

G A G A If a connection is compatible with the metric then the parallel transp ort

preserves the inner pro duct Take an arbitrary p oint A SE Wecanalways nd a

curve which connects A with A Since at A G and G are equal we can cho ose a

basis X for the tangentspace T SE which is orthonormal in b oth metrics The two

i A

metrics have the same connection and we can parallel transp ort vectors X at A to vectors

i



X at A along Since b oth metrics are compatible with the connection and the parallel

i



transp ort preserves the inner pro duct vectors X at A are orthonormal in b oth metrics

i

wo metrics are equal at A But this means that the t

Remark Parallel transp ort along a closed curve which starts and ends at A



SE maps an element X T SE to another element X T SE The collection

A A

of such mappings that we obtain by taking all p ossible closed curves that start and end at A

forms a group called the holonomy group of the connection with the reference p oint A Since

the parallel transp ort preserves the inner pro duct the elements of the holonomy group are

orthogonal transformations with resp ect to any metric compatible with the connection

Wecannow state the second ma jor result of the chapter

Theorem A metric GR d is compatible with the kinematic connection given by

Equation and if and only if it has the form

I

 

GR d

T

R WR



where W is a constant positive denite symmetric matrix

oprove the only if direction

Proof: The if part of the Theorem is just Lemma T

let G b e a metric compatible with the kinematic connection According to Lemma

at the identity the metric G has the form

I

G

M

where M is a p ositive denite smo othly varying symmetric matrix But there exists a



metric G of the form with W M which is equal to G at the identity The two

metrics are b oth compatible with the kinematic connection so according to Lemma

they are the same Metric G therefore has the form

Remark All the metrics of the form are isometric In other words if G

and G are two such metrics there is an isometry b etween SE equipp ed with the

metric G and SE equipp ed with the metric G This isometry do es not preserve

the group structure on SE though The two manifolds are isometric b ecause of the

pro duct structure of the metrics and the fact that anytwo metrics on a Euclidean space

are isometric

Discussion

In this chapter wesawhow SE can b e endowed with additional structure so that

some wellknown results in kinematics can b e interpreted through the geometry of SE

First we show that a natural setting to study screw motions is SE equipp ed with a

metric chosen from a twoparameter family of semiRiemannian metrics These metrics are

indenite and in general they are nondegenerate Viewed as quadratic forms on se the

metrics are linear combinations of the Killing form and the Klein form Any nondegenerate

metric in this family denes the unique symmetric connection for which geo desics are screw

it is a scalar multiple of the motions When the metric is degenerate as a form on se

Killing form the symmetric connection compatible with the metric is no longer unique

and neither are the geo desics However screw motions are still one p ossible set of geo desics

of degenerate metrics derived from the Killing form

To study acceleration or higher order derivatives of the velo city SE must b e equip

p ed with an ane connection The choice of the ane connection is restricted if wewant

to obtain the acceleration of a rigid b o dy bycovariant dierentiation of the velo cityvector

eld Further if we require that the connection is symmetric such connection is unique

In this case a family of Riemannian metrics which are compatible with the symmetric

connection can b e identied All of them are pro duct metrics as a Riemannian manifold

SE is a Cartesian pro duct of SO with the biinvariant metric and IR with the inner

pro duct metric Alternatively the symmetric connection studied in Section can b e

viewed as the symmetric part of a general connection with nonzero torsion asymmetric

part Since geo desics do not dep end on torsion the family of metrics compatible with the

symmetric part of the connection consists precisely of those metrics whichhave the same

geo desics as the connection

Chapter

Task space tra jectory planning

For applications kinematic motion planning is employed more frequently than dynamic

motion planning The main reason is that it is much simpler Kinematic motion planning

will b e sucient as long as there exists a suitable way of computing the actuator forces

from the kinematic tra jectory This is the case for example if there exists a mapping

between the task space and the actuator space We also use kinematic motion planning

when a dynamic mo del of the system is dicult to derive we abstract the dynamic mo del

with a kinematic mo del

Kinematic motion planning can b e p erformed either in the task space or in the joint

space Since the task is sp ecied in the task space it is natural to compute the motion plan

directly in this space Such motion plan will b e in general indep endent of the mechanical

structure of the rob ot In rob otics we are usually interested in smo oth tra jectories They

are preferred b ecause the electromechanical system is limited by its actuator size and its

control bandwidth so it can not pro duce large velo cities and accelerations and b ecause

movements with high acceleration andor jerk can excite the structural natural frequencies

in the system Atypical example is programming of industrial rob ots for tasks such

as welding and painting where a teaching pro cess is employed to record intermediate

p ositions and the nal tra jectory is obtained byinterp olation Similarly in computer

graphics smo othness is required to obtain realistic motions or motions that lo ok natural

when key frames are employed for threedimensional animation Eachkey frame represents

an intermediate p osition and orientation p ose and is sp ecied by the user or programmer

It is then necessary to automatically generate a smo oth tra jectory passing through the key

frames

When comparing dierent metho ds for kinematic motion planning and tra jectories that

these metho ds generate we usually lo ok for the following prop erties

The tra jectories must b e indep endent of the description of the space In this way

computations p erformed with dierentchoices of co ordinates will pro duce consistent

results

The tra jectories should b e indep endent of the choice of the inertial reference frame

fFg and the b o dy xed frame fMg Figure That is it should not matter where

we put the inertial frame and which frame wecho ose on the rigid b o dy the resulting

tra jectory that the rigid b o dy follows should b e the same

The tra jectories must have go o d p erformance for the chosen task

Many commonly used motion planning schemes pro duce tra jectories that dep end on

the choice of the parameterization of the task space An example is shown in Figure

The tra jectories in the gure were obtained by using a straight line interp olation in the

chosen co ordinate space In Figure a we used the canonical co ordinates of the rst kind

dened by Equation In Figure b we used the canonical co ordinates of the second

kind dened by Equation while for Figure c wechose the canonical co ordinates

of the second kind with the order of the basis vectors in Equation reversed The

ob ject moves in the plane z

y y y

14 14 14 12 12 12 10 10 10 8 8 8 6 6 6 4 4 4 2 2 2 0 x 0 x 0 x

5 10 15 20 25 5 10 15 20 25 5 10 15 20 25

a b c

Figure Motion of an ob ject following a straight line in a canonical co ordinates of

the rst kind b canonical co ordinates of the second typ e ordering L L and c

canonical co ordinates of the second typ e ordering L L

Co ordinate indep endence of the tra jectories is assured if the motion planning scheme is

derived within the frameworkofdierential geometry Dierential geometry also provides

a consistentway of extending the notion of dierentiation from Euclidean space to an

arbitrary manifold In this waywe can dene dierent measures of smo othness of the

tra jectories In addition b ecause of the group structure of the task space manifold wecan

establish whether the tra jectories are left or rightinvariant see Sections and

and thus indep endent of the choice of the reference frames

Finding tra jectories on SE that are left and rightinvariant and yield go o d p er

formance turns out to b e dicult if not imp ossible From Corollary it follows that

screw motions are invariant with resp ect to the choice of the inertial and b o dy xed refer

ence frames However according to Corollary screw motions are not geo desics for any

Riemannian metric and are therefore not minimizing anyphysically meaningful distance

Further on SE there is no biinvariant metric This implies that the invariance

of the tra jectories must b e sacriced for p erformance and all we can hop e for is either

indep endence of the tra jectories with resp ect to the choice of the inertial frame fFg or

indep endence with resp ect to the b o dy xed frame fMg

Thereisextensive literature on tra jectory generation in kinematics rob otics and com

puter graphics In order to generate a smo oth motion for a rob otic arm from an initial

to the nal p osition Whitney and Piep er prop osed calculating the screw axis

of the end eector displacement In other words they advo cated a screw motion from

the initial to the nal p osition Waldron develop ed an algorithm that is based on a

slightvariation of Piep ers scheme and prop osed a trap ezoidal velo city prole a constant

acceleration phase followed by a constantvelo city phase and nally a constant deceler

ation phase for the screw motion In all these schemes although the screw motion is

invariant with resp ect to rigid b o dy transformations it do es not optimize a meaningful

cost function Paul decomp oses the desired displacementinto a translation and two

rotations each of which is smo othly parameterized with resp ect to time The motion of

the endeector is obtained by a comp osition of these three displacements He employs

a fourthorder p olynomial of time to obtain a smo oth motion Although there is some

justication for the prop osed tra jectory the approach will lead to dierent tra jectories if

dierent parameterization is chosen for the rotation or if the co ordinate frames in which

the tra jectory is computed are changed There is also no attempt to develop a measure of

smo othness for threedimensional motions

Sho emakeproposedascheme for interp olating rotations in SO with Bezier

curves This idea was extended by Ge and Ravani toSE and prop osed for computer

aided geometric design In b oth cases the interp olating curves are screw motions and

therefore invariant with resp ect to the choice of reference frames However the interp olat

ing scheme pro duces a motion that do es not p osses these invariance prop erties Further

these motions are not of minimal length for any meaningful metric In contrast Park

and Ravani use a scaledep endentleftinvariant metric to design Bezier curves for

threedimensional rigid b o dy motion interp olation

As in the rst part of the dissertation weformulate the motion planning problem as

avariational problem in particular we compute maximally smo oth tra jectories b etween

an initial and a nal p osition and orientation The measure of the lack of smo othness is

chosen to b e the integral over the tra jectory of a cost function dep ending on velo cityorits

higher derivatives Boundary conditions on the derivatives of desired order can b e enforced

by appropriately cho osing the cost function For example by minimizing the norm of the

velo citywe obtain the shortest distance paths The minimumacceleration minimumjerk

tra jectories can b e made to satisfy b oundary conditions on the velo cities accelerations

Dynamically smo oth tra jectories can b e obtained by incorp orating the inertia of the system

into the cost function A simple extension of the ideas in this chapter allows inclusion of

intermediate p ositions and orientations and lends itself to motion interp olation

Necessary conditions for smo oth curves on general manifolds were derived byNoakes et

al and in parallel with our work by Camarinha et al and Crouch and Silva Leite

In necessary conditions for cubic splines which corresp ond to our minimum

acceleration curves are derived for an arbitrary manifold These results are extended in

to the dynamic interp olation problem In necessary conditions for curves minimizing

the integral of the norm of an arbitrary derivativeofvelo city are derived None of these

works deals sp ecically with computing the tra jectories on SE nor do they address the

choice of the metric for the space Since there is no natural metric for SE the

choice of metric for tra jectory planning b ecomes an imp ortant issue

The chapter is organized as follows We start with a discussion on the choice of metric

for tra jectory planning on SE We prop ose a left invariant metric given by the kinetic

energy of a rigid b o dy and derive the expressions for the covariant derivativegiven by this

metric We use these geometric constructs to formalize the ideas of acceleration and jerk

on SE Most of these results are presented here for the rst time We then describ e

the variational problems that need to b e solved in order to calculate the shortest distance

minimumacceleration and minimumjerk tra jectories While some of these results were

derived in and we present alternative pro ofs and sp ecialize the results to SE

Wecontinue by deriving analytical solutions for smo oth tra jectories in some sp ecial cases

At the end of the chapter weshowsomenumerically computed tra jectories to illustrate

their dep endence on the choice of the metric reference frames and b oundary conditions

Choice of metric on SE for motion planning

To measure the length of a vector or a distance b etween twopoints on a manifold we need

a Riemannian metric There is no clear choice of metric on SE In the previous chapter

we obtained two families of metrics by sp ecifying the family of geo desics and enforcing a

particular expression for covariantderivative In this section weareinterested in metrics

that are suitable for motion planning on SE We fo cus on the left invariant metrics see

Section which pro duce tra jectories that are invariant with resp ect to the choice of

the inertial frame but in general change if the b o dyxed frame is changed An example

ofaleftinvariant metric was prop osed byPark and Bro ckett and was discussed in

Section The matrix W in Equation for this metric is given by

I

W

I

are p ositive scalars This metric p ossesses several attractive features where and

By construction the metric is left invariant and do es not dep end on the choice of

the inertial xed reference frame This prop erty is imp ortant for dynamic analysis

of mechanical systems

When restricted to the group of rotations SO it is biinvariant

It preserves the isotropyofIR which means that the metric prop erties of the

space are the same in every direction

For an arbitrary choice of the p ositive constants and the basis vector elds L

i

are orthogonal When they b ecome orthonormal

The metric is compatible with the acceleration connection Prop osition The

Riemannian connection corresp onding to this metric thus yields the acceleration that

is used in kinematics

However metric dep ends on the choice of the two scalars and which act like

scaling factors for angular velo cities and linear velo cities In kinematic analysis there is no

apriorijustication for cho osing them

Another metric that is attractive for tra jectory planning can b e obtained by considering

the dynamic prop erties of the rigid b o dy The kinetic energy of a rigid b o dy is a scalar

that do es not dep end on the choice of the inertial reference frame It thus denes a left

invariant metric For this metric the matrix W in Equation is the inertia matrix and

VV corresp onds to the kinetic energy of the rigid b o dy moving with a velo city V

If the b o dyxed reference frame is attached at the centroid and aligned with the principal

axes then wehave

H

W

mI

where m is the mass of the rigid b o dy and H is the matrix

H

xx

H H

yy

H

zz

with H H andH denoting the moments of inertia ab out the x y and z axes

xx yy zz

resp ectively If f vg is the vector pair asso ciated with the vector V this vector pair

represents the instantaneous twist asso ciated with the motion expressed in the b o dyxed

reference frame The norm of the vector V thus assumes the familiar expression

T T

VV H mv v

is displaced by the matrix Now assume that the b o dy xed frame fMg

R d

C

to a new frame fMg The kinetic energy do es not change if the b o dyxed frame is

C

changed It is not dicult to check that this implies that the matrix W dening the

C

energy metric for the new description of the motion of the rigid b o dy is

T T T

R HR mR D R mR DR

W

C

T

mR DR mI

where D is the skewsymmetric matrix corresp onding to the vector d This is therefore the

most general form of the inertia matrix and can b e viewed as a spatial version of Steiners

parallelaxis theorem

Remark In the App endix A we show that the matrix of metric co ecients G for a

pro duct metric on SO IR induced by a left invariant metric Q on SO and a metric

W on IR has the following form

Q

GR d

T

R WR

when expressed in the basis L Here Q and W are arbitrary p ositivedenite

i

matrices If wesetW I the pro duct metric b ecomes

Q

G

I

The metrics and b oth have this form and are therefore obtained from a pro duct

metric In other words there is an isometry b etween SE endowed with any of these

metrics and the pro duct space SO IR with appropriately dened metrics on SO

and IR respectivelyFurther since any metric of the form is isometric to a metric

of the form any metric induced by the kinetic energy will b e isometric to a pro duct

metric Since none of the functionals that we later use to dene smo othness of a curve

dep end on the group structure of SE calculations in the examples could b e simplied

by p erforming them on the pro duct space SO IR instead However the key results

in this chapter are derived for a general metric and are not limited to pro duct metrics

Hence for the derivations wedonottake advantage of the pro duct structure for SE

Riemannian connection on SE

In this section we nd the Riemannian connections that corresp ond to the left invariant

metrics and We start with an elementary result relating the Christoel symbols

and the structure constants for an arbitrary Lie group It is not dicult to show that

if r is the Riemannian connection then for any three vector elds X Y and Z

fYXZXZYZXY Zr Y

X

Z Y X Z X Y X Y Z g

This immediately implies

Prop osition If r is the Riemannian connection compatible with a left invariant met

ric described by a matrix W w the Christoel symbols for the basis L aregiven

ij i

by

X

k s s s 

w w C w C C w

sj si sm

ji mi mj ij

km

m



k 

where C are the structureconstants of the Lie algebraandw W

km

ij

km

i i

Note that if X X L and Y Y L are anytwovector elds then

i i

i i

dY dY

i j i j k i

L X Y r L X Y L L r Y r L Y

j

i i k j X i

ji

L X L

i j

dt dt

d

k

where is the derivative along the integral curveofX and are obtained from Equation

ji

dt

However for SE and the metrics and more compact expressions can

b e obtained directly from Equation First we prove the following lemma for SE

i i i

L be three arbitrary vector elds and L and Z Z L Y Y Lemma Let X X

i i i

let the corresponding vector pairs be f v g f v gand f v grespectively If r is

x x y y z z

the Riemannian connection corresponding to the leftinvariant Riemannian metric

then

i

Zr Y ZXY L

X i

f v v g f v g

z y z y z y x x

f v v g f v g

z x z x z x y y

f v v g f v g

x y x y x y z z

Starting from this p ointwe use the Einstein summation convention to simplify the notation

ws directly from Equation The Lie bracket of

Proof: The result of the Lemma follo

anytwovector elds is

i j i i

X Y X Y L L X Y L Y X L

i j i i

where X f denotes the action of the vector eld on a scalar function f See Section

Rewritten in terms of the pairs f v g and f v g the rst term b ecomes

x x y y

i j

X Y L L f v v g

i j x y x y x y

Thus in Equation

ZX Y f v g f v v g

z z x y x y x y

i i

ZXY L ZYX L

i i

Furthermore if W are the entries of W in Equation

ij

i j i j i j

XYZ X Y W Z X Y W Z Y W X Z

ij ij ij

i i

L Z YXZ L XY

i i

The result in Equation follows if we similarly expand and add all the terms in

Equation

i i

Prop osition Let X X L and Y Y L be two arbitrary vector elds If r is the

i i

Riemannian connection corresponding to the Riemannian metric then

d dv

y y

r Y f v g

X x y x y

dt dt

d

is the derivative along the integral curve of X The expression is independent of where

dt

the scaling constants and

e employ Lemma and use the metric

Proof: W

i

Zr Y ZXY L

X i

v v v

z y x z y z y x

v v v

z x y z x z x y

v v v

x y z x y x y z

i

v v ZXY L

x y z x y z i

i

v g ZXY L f

x y x y i

where denotes the dot pro duct on IR Since the equation must hold for an arbitrary Z

we can write

i

v g r Y X Y L f

x y x y X i

d

If is the derivative along the integral curveofX

dt

i

dv d dY

y y

i

g L f X Y L

i i

dt dt dt

Hence the result in Equation

i i

L be two arbitrary vector elds If r is the L and Y Y Prop osition Let X X

i i

Riemannian connection corresponding to the Riemannian metric then

d

y

 

H H H H

x y x y y x

dt

r Y

X

dv

y

v

x y

dt

d

where is the derivative along the integral curve of X The translational component of

dt

the expression r Y is independent of the choice of matrix H and thus independent of the

X

choice of the metric on SO e start with Lemma and use the metric

Proof: W

i

Zr Y ZXY L

X i

H m v v v

z y x z y z y x

H m v v v

z x y z x z x y

H m v v v

x y z x y x y z

i

m v v ZXY L

x y z i

H H H

x y z x y z x y z

h

 i

m v v H H H ZXY L

x y z x y z i

i



H H H H

x y z x y z

i

ZXY L Zf

i x y

i

 

H H H H v g

x y y x x y

Since the ab ove is true for an arbitrary Z this proves the prop osition

In the derivation wehave used the fact that H is symmetric and therefore for arbitrary

vectors x and y

T

  

x y x H Hy H x Hy H x Hy

Curvature of SE

In the subsequent sections we will also need the expressions for the Riemannian curvature

of SE By denition the Riemannian curvature tensor RX Y is

RX Y Z r r Z r r Z r Z

Y X X Y

XY

The Riemannian curvature for the metric can b e computed directly in the com

p onents of the corresp onding vector elds

Prop osition If X Y and Z arethree arbitrary vector elds on SE with the asso

ciatedvector pairs f v g f v g and f v gandSE has the Riemannian con

x x y y z z

nection dened in Equation then the Riemannian curvature RX Y Z is

g RX Y Z f

x y z

Proof: See App endix B

Acceleration and jerk of rigid b o dy motions in IR

Knowing how to compute the covariantderivative we are in a p osition to obtain formulas

for the acceleration and jerk Here we use the scaledep endentleftinvariant metric from

Equation Since the connection co ecients and the covariant derivative are indep en

dentofthechoice of constants and the resulting expressions for acceleration and jerk

will also b e indep endent of these scale factors

If V is the velo city tangent to the curve asso ciated with the motion Atofarigid

body the acceleration is the covariant derivativeofV along the curve At If f vg

is the corresp onding velo city pair it immediately follows from Equation that the

acceleration is given by

r V

V

v v

The resulting expression for the acceleration corresp onds to the acceleration that is used

in kinematics This is not surprising given that the metric is compatible with the

acceleration connection see Section

y considering the covariant The third derivative of motion jerk can b e obtained b

derivative of the acceleration along the curve At

d

dt

r r V

V V

dv v

v v

dt

Necessary conditions for smo oth tra jectories

Variational calculus on manifolds

In this section we consider tra jectories b etween a starting and a nal p osition and ori

entation that minimize an integral cost functions while satisfying additional b oundary

conditions on the velo cities andor accelerations The cost functions can b e the kinetic

energy of the rigid b o dy or some other measure of smo othness involving velo cityorits

higher derivatives More sp ecicallywewillbeinterested in curves A a b SE

that minimize integrals of the form

Z

b

dA dA

J h h dt

dt dt

a

where b oundary conditions on At and its derivatives may b e sp ecied at the end p oints

a and b The function h returns a vector eld and usually involves one or more recursive

applications of the covariantderivative We will use a left invariant metric and the cor

resp onding Riemannian connection Unless otherwise sp ecied the scaledep endent left

invariant metric from Equation will b e used

indep endent of the choice of the inertial reference frame fFgwe will use a left invariant

metric and the corresp onding Riemannian connection

We adapt metho ds from the classical calculus of variations to the dierential geometric

setting Noakes et al use such a framework to derive expressions for cubic splines

on a general manifold and they provide the formulas for the group of rotations SO The

cubic splines corresp ond to our minimumacceleration curves and we derive the results from

using more direct approach The necessary conditions for minimumjerk tra jectories

were obtained in parallel with our work and using similar approachby Camarinha et al

In the calculus of variations the rstorder necessary conditions for the minimal solu

tion are derived by studying variations of the optimal tra jectory These conditions take

the form of EulerLagrange equations We will pursue a co ordinatefree formulation of the

EulerLagrange equations which is indep endent of the particular choice of a co ordinate

h the cost func chart We illustrate the basic approach with the simple example in whic

tional is the energy functional where the solution is known to b e a minimal geo desic on

SE

If A a b SE is a piecewise smo oth curve on the manifold a variation of A is a

continuous mapping f a b SE such that

f t At t a b

f is dierentiable on all intervals t t where t t isaninterval on

i i i i

which A is dierentiable

The variation is properiff s a Aaand f s b Ab F or each swe get a

curve t f s t which is called a curve of variation of the curve At The rst

s

requirementabove implies that Atf t On the other hand curves obtained by xing

t therefore s f s t are called transversal curves of variation The tangentvectors

t

d s

t

dene a vector eld along At to the transversal curves at s that is

ds

s

denoted by S called the variational eld of f The tangent along a curveofvariation of

f st

the curve Atis V

t

Let f s tbeavariation of At that is f t At and let the vector elds V and

f st f st

S b e dened by V and S We also require that the curves of variation

t s

f tf s t satisfy the b oundary conditions ie the variation is prop er The value of

s

the cost functional on a curveofvariation tis

s

Z

b

f s t f s t

h dt s J s h

t t

a

If the curve Atf t is a stationary p oint of the ab ove functional then the rst

dJ s

variation must vanish for s In the next subsection we compute the rst variation

ds

of the energy functional and show that the critical p oints as exp ected are geo desics In

subsequent subsections we consider minimization of the L norms of acceleration and jerk

along the tra jectories

Minimumdistance curves geo desics

Park has extensively studied geo desics on SE for the left invariant metric

The main ob jective of considering geo desics here is to illustrate the generality of our

approach and to showhow the approach can b e used to obtain the result in

Consider the energy functional

Z

b

f s t f s t

dt J E s

t t

a

The minimumdistance curves b etween two p oints on the manifold are critical p oints of

f st

this functional Let V The rst variation of E s can b e simplied as follows

t

Z

b

d



E s VV dt

ds

a

Z

b

S VV dt

a

Z

b

r V V dt

S

a

Z

b

r S V dt

V

a

Z

b

VSV Sr Vdt

V

a

Z

b

b

Sr Vdt SV j

V

a

a

The numb ers ab ove the equal signs corresp ond to the following identities that were used

in the derivation

df s

Sf

ds

r S U VSU Sr U

V V

r S r V V S r V since S and V are derivatives with resp ect to co ordinate

V S S

curves t and sV S

R R

df t

b b b

for V V f dt dt f tj

a a a

t dt

The rst and the fourth identity explain howavector eld op erates on a function see

Section The second and the third identity state that r is a Riemannian connection

hence compatible with the metric and symmetric

If a curve is a critical p oint of the functional the rst variation on that curvemust

vanish for an arbitrary variational vector eld S Since the endp oints Aaand Abare

xed S a S b and the rst term in Equation vanishes Therefore if

At f t is a critical p oint

r V

V

dAt

Wethus obtain as exp ected the equation dening a geo desic where V

dt

So far wehave only used the fact that the set of all p ositions and orientations is a

Riemannian manifold Tosolve Equation and nd the geo desics on SE we

express V as a linear combination of left invariantvector elds L L according to

Equation and assemble the co ecients in the vector pair f vg

Prop osition A curve

Rt dt

At

isageodesic on SE equipped with the metric if and only if the vector pair f vg

dA

corresponding to the velocity vector eld V satises the equations

dt

d

dt

dv

v

dt

The second equation in can be simpliedtotheequation

d

e At is a geo desic if and only if Equation is satised Substituting

Proof: A curv

for r V from Equation we obtain the equations in The second equation in

V

can b e written as

v v

T T T

d see Section and using the identity R R and v R By writing R

T T

RR we obtain R

T T T T T

v v v v R d R dR RR d R d

which proves d

Remark It is worth noting that the ab ove result is indep endent of the choice of the

scale factors and The necessary conditions for minimumacceleration and minimum

jerk curves derived in the subsequent subsections will also have the same prop erty

Minimumacceleration curves

We follow the same route as in the previous subsection to compute curves that minimize

the square of the L norm of the acceleration Wederive the necessary conditions by

considering the rst variation of the minimumacceleration functional

Z

b

J r V r Vdt

acc V V

a

dAt

and At is a curve on the manifold The initial and nal p ointaswell where V t

dt

as the initial and nal velo city along the curve are prescrib ed

Theorem Let At be a curve on a Riemannian manifold that satises the boundary

conditions that is it starts and ends at the prescribedpoints with the prescribed velocities

dA

and let V IfAt minimizes the functional J then

acc

dt

r r r V RV r V V

V V V V

Proof: See App endix B

We can directly apply Theorem to SE with the Riemannian connection computed

from the metric

Prop osition Let

Rt dt

At

be a curve between two prescribedpoints on SE that has prescribed initial and nal

dA

the curve minimizes the velocities If f vg is the vector pair corresponding to V

dt

cost functional J derivedfrom the metric only if the fol lowing equations hold

acc

d

n th

where denotes the n derivative of

e start by using Equations and to compute the second term in

Proof: W

Equation

r r r V RV r V V r r r V

V V V V V V V

By rep eated application of Equation the term r r r V can b e simplied and

V V V

the rotational part of the ab ove equation reduces to

After some simplication we arrive at the rst equation in To simplify the trans

lational comp onent we rst observe that the translational comp onentofr V can b e

V

written as see the pro of of Prop osition

T

v v R d

It follows that the translational comp onentofr r V is

V V

d

T T T T T T T T

d R d RR d R d R dR RR d R R

dt

Similarly the translational comp onentofr r r V can b e simplied to get

V V V

T

R d

from which the second equation in directly follows

Remark The ab ove result is indep endent of the choice of the scale factors and

Further the translational comp onent of the necessary condition for minimumacceleration

do es not dep end on the angular motion and the rotational comp onent is indep endentof

the translational motion the latter is clear from the expressions and

Remark As observed in the rst equation can b e integrated to obtain

constant

However this equation can not b e further integrated analytically for arbitrary b oundary

conditions In Section we will showhow to obtain the solution for a sp ecial choice of

initial and nal velo cities

Minimumjerk curves

The minimumjerk curves b etween twopoints are obtained by minimizing the square of L

norm of the Cartesian jerk provided that the appropriate b oundary conditions are given

In particular it is p ossible to solve for minimumjerk tra jectories when the initial and nal

velo cities and the initial and nal accelerations are sp ecied Minimumjerk tra jectories

are particularly useful in rob otics where one is generally able to control the acceleration

of the rob ot end eector and therefore the velo city and the p osition but the electro

mechanical actuators can not pro duce sudden changes in the acceleration It is interesting

to note that Flash and Hogan suggest that humans plan tra jectories that minimize

suchanintegral measure of the jerk when reaching for an ob ject in a horizontal plane and

provide exp erimental evidence in supp ort of this claim

The minimumjerk cost functional is

Z

b

r V r r Vdt J r

V V V jerk V

a

dAt

where as usual V The curvemust start and end at the desired p oints on the mani

dt

fold and with the desired velo cities and accelerations Wearrive at the necessary conditions

for the solution byfollowing the same approach as in the previous two subsections

Theorem Let At be a curve on a Riemannian manifold that satises the boundary

conditions that is it starts and ends at the prescribedpoints with the prescribedvelocities

dA

IfAt minimizes the functional J and the prescribedaccelerations and let V

jerk

dt

then

r V RV r V V Rr V r V V

V

V V V

Proof: See App endix B

The expressions for minimumjerk tra jectories on SE for the metric immedi

ately follow

Prop osition Let At be a curve between two prescribedpoints on SE that has

prescribed initial and nal velocities and initial and nal accelerations If f vg is the

dA

vector pair corresponding to V the curve minimizes the cost functional J for the

jerk

dt

metric only if the fol lowing equations hold

d ws the same reasoning as the pro of of Prop osition and use for

Proof: The pro of follo

mulas and to evaluate the three terms in Equation The rst expression

in Equation was derived using Mathematica

Minimum energy curves

In this subsection wetake the cost functional to b e the kinetic energy of the rigid b o dy

and determine tra jectories that minimize the integral of the kinetic energy over the entire

tra jectory Critical p oints of this functional are the geo desics for the metric Accord

ing to Hamiltons principle in rigid b o dy dynamics for holonomic systems the integral of

the Lagrangian over the entire tra jectory is stationary Therefore the minimization

of the integral of the energy yields a tra jectory consistent with the dynamic equations of

motion when no external forces act on the b o dy

Prop osition Acurve At isageodesic on SE equipped with the metric if

dA

satises and only if the vector pair f vg corresponding to the velocity vector eld V

dt

the equations

d



H H

dt

dv

v

dt

The second equation in can be simplied to the equation

d

e At is a geo desic if and only if Equation is satised Substituting

Proof: A curv

for r V from Equation and letting f v g f v g f vg we get Equation

V x x y y

Since the expression for the translational part is the same as that in Equation

to prove the second part we pro ceed in the same way as in the pro of of Prop osition

Analytical expressions for optimal tra jectories

Shortest distance path on SE

Using prop erties of the Riemannian covering maps Park showed that for the metric

the geo desics on SE can b e obtained by lifting the geo desics from SO and IR

We come to the same result constructively using Equation

Prop osition Given two congurations

R d R d

A A

the shortest distancepath minimal geodesic

Rt dt

At

between these congurations with respect to the metric is given by

Rt R exp t

d t d dt d

where

T

R log R

SeeAppendix F for the denition of the matrix log function The path is unique unless

T

TraceR R when there exist two geodesics of equal minimum length seeRemark

ws from Prop osition The rst equation in can b e readily

Proof: The result follo

integrated to obtain

t

Let b e the matrix equivalent of the vector From Equation wehave

T

R R

Equation can b e thus integrated

T

R R Rt R exp t

From the initial condition weget R R and from the b oundary condition

T

logR R

The expression for the vector dt is obtained byintegrating the equation d twice

As a result weget

dt c t c

and Equation immediately follows from the initial and nal conditions on d

Remark The log function on SO is multivalued If logR yields a solution u

where u is a unit vector along the axis of rotation and is the angle of rotation then for

anyinteger k u k is also a solution This indeterminacy is resolved partially by

restricting to lie in the interval the interval is covered by using the axis

u The geo desic computed by restricting to lie in the interval can b e shown to

give the unique minimallength geo desic unless Ifwe think of SO as a unit

hyp ersphere in IR with antipodal points identied the minimallength geo desic is unique

T

between anytwo general p oints R and R except when TraceR R and there

exist two geo desics of equal minimum length

veled distance b etween two congurations of The ab ove result suggests that the tra



the rigid b o dy will b e minimal only if the p oint O on the rigid b o dy translates along a

straight line with a constant linear velo city while rotating along a xed axis with a constant

angular velo city The geo desic is therefore obtained bycombining geo desics on SO and

IR corrob orating the result in

y y y 20 20 20 18 18 18 16 16 16 14 14 14 12 12 12 10 10 10 8 8 8 6 6 6 4 x 4 x x

5 10 15 20 25 5 10 15 20 25 5 10 15 20 25

a b c

Figure Motions in a plane a a screw motion b a geo desic for metrics and

and c a geo desic after the b o dyxed frame fMg is displaced

Figure shows various tra jectories for a motion in the plane z Figure a shows

a screw motion which in the planar case corresp onds to rotation ab out a xed p oint in the

plane A geo desic for the metric is shown in Figure b For planar motions the

geo desic for the metric will b e the same Since the tra jectory is computed by using a

left invariant metric it do es not change if the inertial reference frame fFg is moved But

the tra jectory changes if wechange the b o dyxed frame fMg The tra jectory is shown in

Figure c and is dierent from the motion in Figure b We also show the motion of

the new b o dyxed frame The gure clearly shows that the new b o dyxed frame follows

a geo desic for metric

It is interesting to compare the shortest distance tra jectory to the tra jectory that mini

mizes the kinetic energy According to Hamiltons principle the tra jectory that minimizes

the kinetic energy is obtained by solving the dynamic equations of motion We stated the

necessary conditions for this tra jectory in Equation The rotational part of Equation

are the well known Euler equations

H H

which in general do not admit an analytical solution However it is trivial to compute



the translational tra jectory of the rigid b o dy The p oint O whichwas chosen to coincide 0 0 0 -5 -5 -5 z z z -10-10 -10-10 -10-10 -15-15 -15-15 -15-15 -20-20 -20-20 -20-20 2020 2020 2020 0 15 0 15 0 15 10 10 10 10 y 10 y 10 y x 20 5 x 20 5 x 20 5

30 0 30 0 30 0

a b c

Figure Tra jectory of an ob ject following a a screw motion b a geo desic for the

metric and c a geo desic for the metric

with the center of mass travels in a straight line with a uniform velo citybetween the initial

and nal p osition of the rigid b o dy This is true for an arbitrary choice of the p ositive

denite matrix H andasaspecialcasecontains the solution for the metric obtained

in Equations and The result is also the geometric statement of Newtons



second law The center of mass of the rigid b o dy the origin O travels in a straight line

if no external force acts on the b o dy

Figures a b and c compare the screw motion the shortest distance tra jectory

given by Equations and and the tra jectory consistent with the dynamic

equations for a homogeneous rectangular prism with sides and units b etween

two given congurations Figure a shows the screw motion of the rigid b o dy b etween

the two congurations It can b e seen that the ob ject rotates ab out a xed axis while

translating along the same axis Figure b shows the motion along the shortest distance

path the geo desic given by Equations and The center of mass of the ob ject

moves along the straight line connecting the initial and nal p osition while the b o dy rotates

around the center of mass as during the screw motion Figure c shows the tra jectory

that the rigid b o dy would follow according to the principles of rigid b o dy dynamics if it

were launched with the appropriate initial velo city and not sub jected to external forces

This is a geo desic for the metric dened in Equation The center of mass again

travels along the straight line but the b o dy rotation is governed by the Euler dynamic

equations The tra jectory was computed numerically by solving a twop oint b oundary

value problem Note that the shap e and mass distribution of the rigid b o dy are only

relevant to the tra jectory in Figure c

Figure shows geo desics on SE for leftinvariant metrics of the form

W

The rows corresp ond to comp onents v and v resp ectively When the metric

z x y

of this form is not a pro duct metric Figure a is the same as Figure b and shows

a geo desic for the pro duct metric when The geo desic is a pro duct of geo desics

on S and IR The other twoguresshow geo desics for the cases when and

the metrics are not pro duct metrics In this case the rotational and the translational

comp onents of motion are coupled In particular the translational motion do es not follow

a straight line These geo desics were computed numerically

y y y

14 14 14 12 12 12 10 10 10 8 8 8 6 6 6 4 4 4 2 2 2 0 x 0 x 0 x

5 10 15 20 25 5 10 15 20 25 5 10 15 20 25

a b c

Figure Geo desics for dierent metrics on SE a pro duct metric b

a metric with and c a metric with

Minimumacceleration and minimumjerk tra jectories

In general the rotational comp onents of the minimumacceleration curves Equation

and minimumjerk curves Equation can not b e computed analytically However

when the initial velo cities and accelerations are collinear with the initial velo city of the

geo desic b etween the two endp oints and the nal velo cities and accelerations are collinear

with the nal velo city of the geo desic it is easy to obtain a solution for these tra jectories

in terms of the geo desic curve If the geo desic curve can b e computed analyticallysocan

minimumacceleration and minimumjerk curves The results in this section are valid for

any geo desically complete manifold

Prop osition Given an initial point q and a nal point q on a Riemannian manifold

let bea geodesic connecting these two pointssothat q and

d d

q Let V and V If the boundary conditions for the minimum

dt dt

t t

acceleration curve are of the form

V t V V t V

then the minimumacceleration curve is given by ptwhere pt is a thirddegreepoly

nomial that satises

p p

 

p p

dp



where p

dt

umacceleration curve has the form t pt where

Proof: Assume that the minim

d

d



p is an arbitrary scalar function p IR IR It is easy to see that V p Let

dt dt

d

Since is a geo desic r T It then follows T

T

dt

   

r V V p T p p r T V p T

V T

   

But V p isaderivativeofp along soV p p It immediately follows that

n n

r V p T

V

Using the linearity of the curvature we also get

   

RV r V Rp T p T p p RT T

V

Equation therefore reduces to

p T

and since T is a tangentvector for a geo desic and therefore never vanishes wemust have

p

Solution of this dierential equation is a p olynomial of degree and the b oundary condi

tions transform into Equation

An analogous argument can b e used to prov e the following prop osition

Prop osition Given an initial point q and a nal point q on a Riemannian man

ifold let bea geodesic connecting these two points so that q and

d d

q Let V and V If the boundary conditions for the minimumjerk

dt dt

t t

curve are of the form

V t V V t V

V V r V j r V j

V V

t t

then the minimumjerk curve is given by ptwhere pt is a fth degreepolynomial

that satises

p p

 

p p

 

p p

For this sp ecial form of the b oundary conditions the minimumacceleration minimum

jerk and minimumdistance paths are therefore the same only the parameterization along

the path varies The shortest distance geo desic the minimumacceleration tra jectory and

the minimumjerk tra jectory when and for SE equipp ed with

the metric are illustrated in Figure The path followed by the rigid b o dy in all

three cases is the same The minimumacceleration tra jectory starts and ends with zero

velo city so the velo city starts from zero rises to a p eak and then decreases to zero The

rise to p eak and the fall to zero are steep er in the minimumjerk tra jectory b ecause the

starting and ending accelerations are also zero

Figure shows that for more general b oundary conditions the path of the minimum

acceleration curve do es not followageodesicFurther the path changes with the b oundary

conditions The gure shows minimumacceleration motions in plane for dierentchoices

of the initial and nal velo cities again for SE equipp ed with the metric In Figure 0 0 0 -5 -5 -5 z z z -10-10 -10-10 -10-10 -15-15 -15-15 -15-15 -20-20 -20-20 -20-20 2020 2020 2020 0 15 0 15 0 15 10 10 10 10 y 10 y 10 y x 20 5 x 20 5 x 20 5

30 0 30 0 30 0

a b c

Figure Tra jectories for zero b oundary conditions a the shortest distance path b

the minimumacceleration motion and c the minimumjerk motion

y y y

14 14 14 12 12 12 10 10 10 8 8 8 6 6 6 4 4 4 2 2 2 0 x 0 x 0 x

5 10 15 20 25 5 10 15 20 25 5 10 15 20 25

a b c

Figure Minimumacceleration motions in plane for general b oundary conditions a

T T T

V V f g b V f g V f g and c V

T T

f g V f g

a the initial and nal velo cities are so the ob ject follows the geo desic path shown in

Figure b but with a dierentvelo city prole The initial and nal velo cities for Figs

bc are not collinear with the initial and nal velo cities of the geo desic in the gure

b and it is clear the paths are dierent from the previous case On the gure only the

comp onents of the velo cities in the plane are given V f v v g

z x y

The next gure shows threedimensional minimumacceleration motions for gen

eral b oundary conditions Similarly to the previous example minimumacceleration curves

do not follow a geo desic The gure also demonstrates that the tra jectories change con

siderably with b oundary conditions Because of the pro duct structure of the metric

whichwas used to compute the tra jectories the rotational motion is indep endent from the

translational motion However the translational comp onent will dep end on the rotational

comp onent Boundary conditions for the motion in Figure a are zero so the motion fol

lows the geo desic path The motion in Figure b starts and ends with large translational

velo city and small rotational velo city For the motion in Figure c the translational

velo cities at the initial and nal p oints are smaller but the rotational velo city is large 0 0 0 -5 -5 -5 z z z -10-10 -10-10 -10-10 -15-15 -15-15 -15-15 -20-20 -20-20 -20-20 2020 2020 2020 0 15 0 15 0 15 10 10 10 10 10 10 y y y 20 5 5 x 5 x 20 x 20

30 0 30 0 30 0

a b c

T

Figure Minimumacceleration motions in space a V V f g

T T

b V f g V f g and c V

T T

f g V f g

Comparison b etween kinematic and dynamic motion planning

We conclude the chapter with comparison b etween the motion plans computed with kine

matic and dynamic motion planning Figure shows a minimumjerk tra jectory and

minimum torquechange tra jectories that were computed using the mo del from Chap

ter In all three cases the initial and nal velo cities and accelerations were set to

Minimumjerk tra jectory shown in Figure a do es not dep end on the mechanism to

which the ob ject endeector is attached and follows a straight line For the minimum

torquechange tra jectory in Figure b we did not imp ose any b oundary conditions on

the preload forces The translational tra jectory of the ob ject in this case is quite curved

and the ob ject also rotates dierently than in the previous case For the tra jectory in Fig

ure c the initial preload force was N while the nal preload force was not sp ecied

The translational motion of the ob ject is now closer to the straight line although it is still

slightly curved Also the rotation of the ob ject is similar to the minimumjerk motion see also Figure

y y y

0.125 0.125 0.125 0.1 0.1 0.1 0.075 0.075 0.075 0.05 0.05 0.05 0.025 0.025 0.025 0 0 0 x x x

-0.2 -0.15 -0.1 -0.05 0 -0.2 -0.15 -0.1 -0.05 0 -0.2 -0.15 -0.1 -0.05 0

a b c

Figure Kinematic vs dynamic motion planning a minimumjerk b minimum

torquechange preload forces not sp ecied and c minimum torquechange initial

preload force N

Discussion

This chapter addressed the problem of generating smo oth tra jectories for a rigid b o dy

between an initial and a nal p osition and orientation The main idea was to dene a func

tional measuring the smo othness of a tra jectory and nd a tra jectory that minimizes this

cost functional Using dierential geometry the problem was formulated as a variational

problem on the Lie group of rigid b o dy displacements SE We dened an inner pro duct

on the Lie algebra se leading to a left invariant Riemannian metric on SE This met

ric gave rise to a Riemannian connection and a covariant derivative Wederived analytical

expressions for the covariantderivative and the curvature of SE The covariantderiva

tivewas used to dene acceleration and jerk for spatial rigid b o dy motions We stated

the necessary conditions for minimumdistance minimumacceleration and minimumjerk

tra jectories and sp ecialized these conditions for SE We computed the analytical so

lutions for minimumdistance tra jectories bycho osing an appropriate basis for the space

of the vector elds We also found analytical solutions for the minimumacceleration and

minimumjerk tra jectories for a sp ecial class of b oundary conditions We provided several

numerical examples to illustrate how the generated solutions are aected by a the met

ric b the choice of the b o dyxed reference frame and c the b oundary conditions A

simple extension of the ideas in this chapter allows the inclusion of intermediate p ositions

and orientations and lends itself to motion interp olation see The presented metho ds

also have applications in computer graphics and computeraided design

Chapter

Concluding remarks

One of the advantages of rob ots over xed automation is that they are versatile and can b e

programmed for a variety of tasks But to make a rob ot suciently dextrous to p erform

dierent tasks it must b e equipp ed with multiple degrees of freedom Co ordination of

these degrees of freedom requires motion planning

The problem of nding a suitable motion for a given task is usually underdetermined

We argued that in such cases it makes sense to dene the p erformance of a motion for

the task so that dierent motions can b e compared This enabled us to treat motion

planning as a variational problem where the motion plan is obtained by maximizing the

chosen measure of p erformance This metho d is continuous in the sense that the motion

planning problem is not discretized To further motivate the chosen approachwe presented

an investigation of human reaching motions The study suggested that tra jectories used

byhumans minimize a certain cost functional We dened dynamic and kinematic motion

planning Dynamic motion planning is used when the actuator forces must b e provided as

part of the motion plan kinematic motion planning is simpler and is used when it suces

to compute the kinematic tra jectory in the task space or joint space

The metho d that we prop osed for dynamic motion planning consists of nding a motion

that satises the dynamic equations of the system and maximizes the cost functional

measuring the p erformance of the motion for the task Limits on the capabilities of the

system and task requirements restrict the set of feasible motions The resulting set can in

general b e describ ed with equality and inequality constraints To compute a motion plan

wedevelop ed a novel numerical metho d for minimizing a given cost functional sub ject to

equality and inequality constraints The metho d was used to nd smo oth tra jectories and

actuator forces for two planar co op erating manipulators holding an ob ject It was then

extended for systems that change the dynamic equations as they move The example of a

simple grasping task illustrated that for such systems variational approach unies motion

planning and task planning

An imp ortant feature of the prop osed metho d is that planning in the task space joint

space and actuator space is p erformed in one step In this way constraints at all three

levels can b e incorp orated into the motion planning pro cess Further kinematic motion

planning is not separated from planning the actuator tra jectory Since it is in general

necessary to plan the kinematic as well as the actuator tra jectory nding the tra jectories

separately is unnatural and can result in p o or motion plans Finally motion plans obtained

with the variational metho ds will globally maximize p erformance and will thus b e sup erior

to motion plans computed with other metho ds

Kinematic motion planning is simpler and more ecient than dynamic motion planning

It is thus preferred to dynamic motion planning if there exists a suitable way to compute

the actuator forces from the kinematic tra jectory Kinematic motion planning is also the

only feasible alternative when a dynamic mo del of the system is to o complicated or is

not available and is abstracted with a kinematic mo del Weformulated kinematic motion

planning as a variational problem on a Riemannian manifold In this way the computed

tra jectories do not dep end on the description of the space Using an ane connection we

dened the notions of acceleration and jerk on an arbitrary manifold The ane connection

can b e chosen arbitrarily which implies that there is no intrinsic notion of acceleration and

jerk We also showed that a Riemannian metric must b e intro duced on the manifold to

dene cost functionals that measure the smo othness of a curve

The prop osed metho d for kinematic motion planning can b e used either in the task

space or in the joint space However the task space has more structure and we take

advantage of this structure to nd a Riemannian metric In most cases the task space is

the group of spatial rigid b o dy motions SE We used the group structure of SE

to dene metrics that have certain invariance prop erties with resp ect to the choice of the

co ordinate frames describing the motion in the task space In this pro cess we also identied

metrics and connections on SE that are useful for kinematic analysis

By cho osing the class of explicit metho ds for motion planning we assumed that con

trollers to follow the computed tra jectories can b e synthesized If the system is controllable

such controllers can readily b e obtained We also assumed that wehave a complete de

terministic mo del of the system so that variational metho ds can b e used Such mo dels are

available in many rob otic applications so this assumption is not overly restrictive

Summary

In the rst chapter we reviewed the literature on motion planning in humans and existing

approaches to motion planning for rob otic systems The main deciency of most of these

metho ds is their inabilitytoprovide a set of actuator forces that are consistent with the

constraints imp osed by the task and limited capabilities of the rob ot This motivated

formulating the motion planning problem as a variational calculus problem In the next

chapter we discussed optimal control and its relation to variational calculus Weshowed

that when the constraints on the controls can b e describ ed with equalities and inequalities

the optimal control problem can b e transformed into a problem of Bolza in the calculus of

variations We also constructed an unconstrained variational problem that has the same

set of extremals as the problem of Bolza

Equality and inequality constraints will b e presentinany realistic motion planning

problem The most imp ortant criterion in cho osing the numerical metho d for solving the

motion planning problem formulated as a problem of Bolza was therefore howwell the

metho d handles such constraints As the inequality constraints lead to corner p oints most

of the metho ds based on the minimum principle or EulerLagrange equations are dicult

to apply In Chapter we presented twonumerical metho ds that can deal with inequality

constraints The rst metho d was prop osed by Gregory and Lin This metho d avoids

problems with corners by using the integral form of the necessary conditions By a pro

cedure that resembles niteelement analysis these necessary conditions are transformed

into a set of nonlinear algebraic equations in the values of the unknown functions at dis

crete mesh p oints Weprovided a detailed analysis of this metho d for the case of equality

and inequality constraints that only dep end on the state Weshowed that statevariable

inequality constraints lead to a singular problem which means that the numerical solutions

can exhibit oscillations We next describ ed a new metho d for nding a lo cal minimum for

the problem of Bolza The metho d was based on approximating the continuous problem

with a sequence of nitedimensional nonlinear programming problems through collo cation

We describ ed the metho d of augmented Lagrangian for dealing with the constraints of a

nonlinear programming problem and Newtons metho d for minimization We also showed

that the choice of collo cating functions motivated by niteelement analysis leads to simple

expressions for the gradients that are needed for minimization of the cost functional We

next discussed the choice of a cost functional for the motion planning problem Weshowed

that dep ending on the cost functional we get a kinematic or a dynamic motion planning

metho d We concluded the chapter with a discussion on cost functionals that can b e used

to obtain smo oth motion plans

The numerical metho d of Gregory and Lin was used in Chapter to compute smo oth

motion plans for two co op erating manipulators holding an ob ject Co op erative tasks

are characterized by closed kinematic chains and typically result in actuator redundancy

Based on the consideration of smo othness requirements wechose to plan tra jectories that

minimize the integral of the square norm of derivatives of actuator forces In this wayitis

p ossible to satisfy b oundary conditions on the acceleration and obtain a smo oth transition

from one motion to another Wesolved the motion planning problem when the ob ject

was grasp ed rigidly and when a frictionassisted grasp was used to manipulate the ob ject

When a frictionassisted grasp is used the forces must satisfy frictional constraints Us

ing the chosen numerical metho d w e computed typical motion plans for b oth cases The

numerical metho d proved to b e quite robust and ecient Results from this chapter are

rep orted in and

In Chapter weshowed that principles used for rob ot motion planning can serveasa

mo del for motion planning in biological systems We presented exp erimental measurements

of human twoarm tra jectories A qualitative analysis of these data was p erformed with

sp ecial fo cus on the internal forces These forces indicate what co ordination pro cesses take

place b etween the two arms during the co op erative task Wewere not able to obtain a

consistent pattern except when the motion to ok place in the sagittal plane and the task was

symmetric for b oth arms In this case the kinematic features of the measured tra jectories

and the general shap e of the interaction forces were well mo deled with the tra jectories that

minimize the minimum torquechange criterion This work is describ ed in and

The next chapter intro duced motion planning problem for systems that change the

dynamic equations as they move In rob otics examples of such systems are a multingered

hand manipulating an ob ject or a legged machine walking on the ground We showed that

the motion planning problem for such systems is quite complex However the variational

formulation of the problem unies motion planning and task planning We develop ed a

technique for computing motion plans when the sequence of discrete states that the system

traverses is known in advance In such case the planning task is to determine continuous

tra jectories for the system and the optimal times when the system should switch from one

discrete state to another We argued that the sequence of discrete states can b e computed

in advance in some imp ortant rob otic tasks The metho d was illustrated by computing

motion plans for a twongered hand rotating a circular ob ject In this case it was essential

to use the newly develop ed numerical metho d from Chapter to obtain the solution The

main ideas from this chapter are rep orted in

The next twochapters were devoted to planning smo oth tra jectories in the task space

Smo oth tra jectory in the task space is required for example to move through a sequence

of p oints that were recorded byateaching pro cess in the task space Task space can b e

represented with the sp ecial Euclidean group SE hence we started byinvestigating

the geometric prop erties of this group In particular we studied choices of metrics and

connections on SE We established that there is no Riemannian metric which has

screw motions for geo desics We next identied a symmetric connection which leads to

kinematically meaningful acceleration and found a family of Riemannian metrics which

are compatible with this connection Publications describing the work in this chapter are

and

Wedevelop ed a metho d for computing smo oth tra jectories on the task space mani

fold in Chapter We argued that by using a geometric framework tra jectories that are

indep endent of the representation of the task space can b e obtained Consequently the

tra jectory planning problem was formulated as a variational problem on a Riemannian

manifold Weshowed that a Riemannian metric which also gives rise to a unique con

nection is necessary to dene the notions of the acceleration and jerk Results from the

previous chapter were used to nd the necessary conditions for tra jectories that minimize

energy square of the L norm of the acceleration and square of the L norm of the jerk

ed for some sp ecial cases on SE Analytical expressions for the tra jectories were deriv

Wealsoprovided several numerical examples to illustrate how the generated tra jectories

are aected by the metric the choice of the b o dyxed reference frame and the b oundary

conditions The results from this chapter are rep orted in and

Contributions

Contributions of this dissertation fall into several areas

We prop ose a unied framework for motion planning The underlying idea is to use

the metho ds of calculus of variations In this framework tra jectories in the task

space joint space and actuator space are obtained in a single step while simultane

ously taking into account limitations of rob otic mechanism and task constraints The

validity of the approach is demonstrated by solving several complex motion planning

problems

Central to the application of the metho ds of variational calculus is an ecientnumer

ical metho d Wedevelop a new numerical metho d for solving variational problems

using the ideas from nonlinear programming and niteelement analysis An im

p ortant feature of the metho d is that the gradient of the cost functional is easy to

compute As a result the metho d is ecient and easy to implement We also present

a detailed analysis of the numerical metho d develop ed by Gregory and Lin and

suggest a numb er of improvements

A technique for solving variational problems when the dynamical system go es through

a known sequence of transitions b etween regions representing dierent dynamic b e

havior is derived Previous metho ds used iteration on the unknown switching times

requiring a solution of an optimal control problem at each iteration Instead wesolve

the optimal control problem only once byintro ducing additional state variables This

metho d is general and do es not dep end on the numerical metho d used for solving

the optimal control problem

By describing tasks such grasping and walking as a system that changes dynamic

equations when it moves we are able to unify motion planning and task planning

We therefore establish a link b etween motion planning metho ds and algorithms for

task planning While wedonothave a complete solution for the generalized motion

planning problem our formulation provides a framework for further investigation of

this problem

We present exp erimental investigation of human twoarm manipulation and prop ose

the minimum torquechange mo del to account for the observed tra jectories This

is the rst attempt see also to mo del tra jectory generation for twoarm mo

tions While the prop osed mo del has considerable shortcomings it predicts kinematic

features of the tra jectories and force proles for sagittal plane motions whichare

characterized by a symmetric role of the two arms The study suggests that the role

of the twoarmsintwoarm reaching tasks is not symmetric This observation can

serve to formulate a criterion that b etter describ es human motion planning

Wedevelop a practical metho d for task space tra jectory planning using the geometry

of the task space Most other metho ds assume Euclidean structure of the task space

for computations or are limited to theoretical results and hard to implement We

SE for some sp ecial but provide analytical solutions for optimal tra jectories on

imp ortant cases The tra jectories generated by the prop osed metho d are indep en

dent of the description of the task space Wealsoshowhow some further invariant

prop erties can b e obtained by an appropriate choice of a metric for SE

We attach geometric meaning to several concepts from kinematics and present some

new results regarding the choice of metrics and connections for kinematic analysis

In particular we prove that there is no Riemannian metric for which screw motions

are geo desics Further weshow that an ane connection must b e chosen to dene

the acceleration and identify the connection that pro duces a physically meaningful

acceleration

Possible applications and future work

While wehave made considerable progress in applying variational metho ds to kinematic

and dynamic motion planning for rob otic systems the work op ened several avenues for

future research Traditional approaches to motion planning are based on discretized rep

resentation of the conguration manifold and graph search techniques In contrast the

basis for many continuous tra jectory planning metho ds are optimization techniques Fur

ther advances in motion planning can b e exp ected if partial solutions of the continuous

problem are used as building blo cks for the discrete metho d In this way it is p ossible to

construct quasioptimal solutions By employing optimization metho ds for lo cal planning

there is also no need for constructivecontrollability algorithms that are dicult to derive

for systems with constraints

In the dissertation wehave not addressed the problem of tracking the tra jectories

pro duced by the planning pro cess While the results in the cases when the system is

controllable are well known in control literature such metho ds fail for systems that are

sub ject to unilateral constraints and therefore need further investigation

Design and control of hybrid systems where continuous and discrete b ehaviors co ex

ist is a rapidly growing area in control The optimal control metho d develop ed in the

dissertation represents an attempt to design op enlo op tra jectories for a subset of hybrid

systems where the discrete b ehavior corresp onds to switches b etween regions of the state

space characterized by dierent sets of dynamic equations When the discrete switches

are controlled by another pro cess a problem b ecomes one in game theory and the optimal

control metho d must b e appropriately extended This extension seems feasible since game

theory is a natural extension of optimal control Manufacturing control of interaction

between a human op erator and a virtual or a mechanical environment and co ordination of

multiple agents are some p ossible applications where such metho d could b e used

Another area to which this work could b e extended is mo deling and control of mechani

cal systems sub ject to unilateral and nonholonomic constraints Nonholonomic constraints

can b e relatively easily handled in optimal control and numerical solutions for optimal tra

jectories can b e readily computed Theoretically suchvariational problems are studied in

subRiemannian geometry and applications of this theory to control are still rare Through

e prop er extension of Hamiltons principle variational calculus also provides an alternativ

to mo deling dynamical systems sub ject to inequality constraints that app ear for example

in contact mechanics

Study of metrics and ane connections on the group of rigid b o dy motions SE

provides some new insights in the theory of screws and its use in kinematics These results

can b e further formalized and have consequences for the formulation of the p ositionforce

control schemes An imp ortant op en question is also the existence of analytical solutions

for minimum acceleration and minimum jerk tra jectories Such expressions would b e of

considerable value for applications

Initial motivation for this researchhave b een studies of human tra jectory generation

Knowing howhumans move can aid in developing motion planning algorithms On the

other hand metho ds that were primarily designed for rob ot tra jectory planning can also

b e used to mo del human motor b ehavior The two areas therefore complement each other

and it is imp ortant to further our understanding of human motor b ehavior A p ossible area

of application of this work is also computer animation where understanding of principles

that govern human motion and go o d techniques for planning actuator forces are essential

for generating a naturally lo oking animated gure

App endix A



Pro duct metric in the basis fL g

i

In this section we derive the expression in the basis fL g for the pro duct metric on SE

i

obtained by taking an arbitrary leftinvariantmetricQ on SO and an inner pro duct

metric on IR given by a constant p ositivedenite matrix W If the basis L L

and L is chosen for the vector elds on SO and the Euclidean basis E E E for the

vector elds on IR the matrix G describing the metric in this basis has the form

Q



A G

p

W

E E in the To compare metric A with wehave to express the vectors E

basis L L

Take a p oint A SE where

R d

A A

Note that A as an elementofSO IR is represented by a pair R d Accordingly

SE is parameterized as a pro duct manifold SO IR with the usual parameterization

of IR any parameterization of SO can b e chosen Takeavector eld

L

T

X X L X L X L v A

L

L

T

where the comp onents X X and X are constant and v fX X X g The integral

curve of this vector eld passing through A is given by t A exp tS where S se

is the matrix representation of the vector s f X X X g It is easy to see that

R tR v d

t A

t tg is given by The tangentvector to a curve t f

d d

i

A

dt dt

i

We rst note that on the curve t the rotational part is constant which means that the

d

i

for i are all Further since the parameterization of SE is co ecients

dt

induced by the parameterizations of SO and IR wehave

tR v d A

The expression for the tangentvector X A in the basis E is therefore

i

i

L

E

T T

v E Rv A

L

E

L

Since the equation must b e true for arbitrary v weget

L

E

T

A R E

L

E

L

The last expression also implies that change of the basis vector elds on IR from E E

L will only change the lowerright blo ck of matrix G The entries and E to L L and

in this blo ck are obtained by

L

E

h i h i

T T

E

E E E R R WR A R

L L L L

E

L

def

EE where E E

i j i j

App endix B

Pro ofs

B Pro of of Prop osition

In the pro of we will use the following conventions

def

x

i

X X L

i

v

x

X

def

y

i

X Y L

i

X v

y

and

def

XY

X Y

v

XY

By denition the Riemannian curvature is given by

RX Y Z r r Z r r Z r Z

Y X X Y

XY

Using the expressions derived in Prop osition one obtains

X Y

x z y z z z

RX Y Z r r

Y X

X v v Y v v

z x z z y z

X Y

z z

XY

X Y v v

z z

XY

Y Y X YX

x z x z y z y x z z

YXv Y v Y v X v v

z x z x z y z y x z

XY X X Y

z y z y z x z x y z

XY v X v X v Y v v

z y z y z x z x y z

X Y X Y

z x y y x z

X Y v X Y v

z x y y x z

x y z

B

Note that the nal expression only dep ends on the values of the vector elds X Y and

Z and not on their derivatives For this reason the Riemannian curvature can b e used to

dene the so called curvature tensor

B Pro of of Theorem

The pro of follows the same lines as the derivation of the geo desic equation In

addition to the identities from Section we will use an additional identity

r r U r r U RT SU

S T T S

which is just the denition of the curvature op erator when S T and one of the

symmetry prop erties of the curvature tensor

RX Y Z T RZ T X Y

In the pro of b elow the numbers over the equal sign indicate which identities were employed

Z

b

d



L s r V r Vdt B

V V

a

ds

a

Z

b

r V r Vdt S

V V

a

Z

b

r r V r Vdt

S V V

a

Z

b

r r V RV SV r Vdt

V S V

a

Z

b

r r S r V RV r V V S dt

V V V V

a

Z

b

Vr S r V r S r r V RV r V V S dt

V V V V V V

a

Z

b

b

r S r Vj VSr r V Sr r r V

V V V V V V V

a

a

RV r V V S dt

V

b

r S r V Sr r V

V V V V

a

Z

b

Sr r r V RV r V Vdt

V V V V

a

Every curveofvariation must satisfy the b oundary conditions Since p ositions and

velo cities are prescrib ed at t a and t bbothS and r V r S vanish at the

S V

endp oints This implies that the integrated part the rst two terms in Equation B

equals Furthermore on the critical p oint the integral must vanish for any admissible

variational eld S which means that

r r r V RV r V V B

V V V V

B Pro of of Theorem

Once again we employ identities from Section and from Section B

We rst obtain the expression for the rst variation of the functional L

j

Z

b

d



L r s V r Vdt B

j V V

ds

a

Z

b

S r V r Vdt

V V

a

Z

b

r r V r Vdt

S

V V

a

Z

b

r r r V RV Sr V r V dt

V S V V

V

a

Z

b

Vr r V r V r r V r V Rr V r V V S dt

S V S V V

V V V

a

Z

b

b

r r V RV SV r r r V r V V

V S S V

V V

a

a

Rr V r V V S dt

V

V

Z

b

b

r r V r V r S r V RV SV r V

S V

V V V V

a

a

V V S dt r Rr V

V

V

Z

b

b

r r V r Vr S r V V r S r V

S V V V

V V V

a

a

V V S dt V V S Rr V r RV r

V

V V

b

V r S r r r V RV SV r V

V V S

V a V

Z

b

r S r V RV r V V S Rr V r V V S dt

V V

V V V

a

b

r r S r V RV r V V S r S r V

V V V

V V V a

Z

b

VSr V Sr V RV r V V Rr V r V V S dt

V

V V V V

a

b

r r S r V RV r V V S r S r V Sr V

V V V

V V V V a

Z

b

r V RV r V V Rr V r V V S dt

V

V V V

a

Since the initial and nal p ositions velo cities and accelerations are xed S r V r S

S V

and r r V r r V RV SV r r S vanish at the endp oints Thus the integral

S V V S V V

in the ab ove equation must vanish for an arbitrary variation that preserves the b oundary

conditions But this is only p ossible if Equation holds so the Theorem is proved

App endix C

Metric with screw motions as

geo desics

In Section we concluded that Equation

X

l l

C g C g C L g

lj li k ij

ki kj

l

k

must b e satised by the metric if screw motions are geo desics The co ecients C are

ij

the structure constants of the Lie algebra se see App endix E Weevaluated this

equation in Mathematica to obtain a system of partial dierential equations that have

to b e solved for the metric co ecients g In the equations we use the abbreviation

ij

def

k

G L g

k ij

ij

G G g G g

g g G G G

g G g G g g G

G g G g G g g

g G g g G g G

G g G g g G g

g g G g g G G

G G g G g

G g G g G g g

g G g G G

G g G g g G g

G G g G g

G g G G g

G g G G g

G g g G g G g

g g G g G g G

G g G g G g g

g G G g G

G g g G G g g

g G G g G

G g g G g G g

g G G g G

G g G g G

G g G g G

G g G g g G g

G g G g G

g g G g G g G

G g G g G

C

g g G g g G G

g G g G G

G G g G g

G G G

G g G g G g g

G G G

G g G g g G g

G G G

g G g G G

G G G

G g g G g G g

G G G

G g G g G

G G G

App endix D

Metric compatible with the

acceleration connection

In Section we concluded that a metric compatible with the acceleration connection

must satisfy Equation

X

l l

g D g L g

li lj k ij

jk ik

l

k

The Christoel symbols sp ecify the acceleration connection and are listed in

ij

j

are and therefore L g For this reason For k the Christoel symbols

k ij

ik

we only list equations for k The equations were generated in Mathematica and are

k

listed b elow In the equations G stands for L g

k ij

ij

G G g G g

g G g G g g G

G g G g g G g

g g G g g G G

g G g g G g G

G g G g g G g

G g G G g

D

G g g G g G g

G g G g G g g

g g G G g g G

g g G g G g G

G g G g G

g G g g G g G

G g g G g G g

G g g G g g G

G G g G g

G g G g G g g

G g G g g G g

G g G G g

G g g G g G g

G g G g G

App endix E

Lie brackets for se


In our derivations we need to evaluate Lie brackets of the basis vector elds L According

i

L are left invariant it suces to evaluate the to Equation since the vector elds

i

brackets on se From Equation we obtain

L L L L L L L L

L L L L L L L L

L L L L L L L L

L L L L L L L E

L L L L L L L L

L L L L L L

L L L L L L

App endix F

The logarithm function on SO


and SE


The function log SO so is dened by

T

R R F log R cos TraceR

sin

whereisa skewsymmetric matrix such that the corresp onding vector has

unit Euclidean norm and is a real numb er Geometrically corresp onds to the unit

vector along the axis of rotation while is the angle of rotation Note that the log function

on SO is multivalued

The log function on SE log SE se is dened by

R d v

logR log

and



sin j j j j cos j j

v I d

sin j j

where in the last expression is the vector corresp onding to the skewsymmetric matrix

Biblio graphy

A Ailon and G Langholz On the existence of timeoptimal control of mechanical

manipulators Journal of Optimization Theory and Applications May

CGAtkeson and J M Hollerbach Kinematic features of unrestrained vertical arm

movements The Journal of Neuroscience

F Avnaim J D Boissonnat and B Faverjon A practical exact motion planning

algorithm for p olygonal ob jects amidst p olygonal obstacles Technical Rep ort

INRIA SophiaAntip olisFrance

RSBall The theory of screwsHodgesFoster Dublin

R Bellman Dynamic Programming Princeton University Press Princeton NJ

LDBerkovitz Variational metho ds in problems of control and programming

Journal of Mathematical Analysis and Applications

D P Bertsekas Constrained optimization and Lagrange multiplier methods Aca

demic Press New York

G Bessonnet and J P Lallemand Planning of optimal free paths of rob otic ma

nipulators with b ounds on dynamic forces In Proceedings of International

ConferenceonRobotics and Automation pages Atlanta GA

R L Bishop and S I Goldb erg Tensor Analysis and ManifoldsDover Publications

New York

E Bizzi N Accornero W Chapple and N Hogan Posture control and tra jectory

formation during arm movement The Journal of Neuroscience

G A Bliss Lectures on the calculus of variations University of Chicago Press

Chicago st pho enix edition

AMBloch P S Krishnaprasad J E Marsden and R M Murray Nonholo

nomic mechanical systems and symmetryTechnical Rep ort CDS California

Institute of TechnologyPasadena CA

JE Bobrow S Dub owsky and JS Gibson Timeoptimal control of rob otic ma

nipulators along sp ecied paths Int J Robotic Research

JD Boissonnat O Devillers and S Lazard Motion planning of legged rob ots

In K Goldb erg D Halp erin JC Latomb e and R Willson editors Algorithmic

Foundations of Robotics pages A K Peters Ltd Wellesley MA

M Brady J M Hollerbach T L Johnson T LozanoPerez and M T Mason

Robot motion Planning and control MIT Press Cambridge MA

M S BranickyVSBorkar and S K Mitter A unied framework for hybrid

control In Proceedings of the rd IEEE ConferenceonDecision and Controlpages

Lake Buena Vista FL

M S Branicky and S K Mitter Algorithms for optimal hybrid control In Proceed

ings of the th IEEE ConferenceonDecision and Control pages New

Orleans LA

R W Bro ckett Control theory and singular Riemannian geometryInP Hilton and

G Young editors New directions in Applied Mathematics pages Springer

Verlag New York

RWBrockett Rob otic manipulators and the pro duct of exp onentials formula In

oc Symp Math Theory Networks and Systems pages Beer Sheba Israel Pr

RWBrockett Hybrid mo dels for motion control systems In H L Trentelman

and J C Willems editors Essays in Control Perspectives in the Theory and its

Applications pages Birkhauser Boston

R A Bro oks Solving the ndpath problem by go o d representation of free space

IEEE Transactions on Systems Man and Cybernetics SMC

A E Bryson and YC Ho Applied Optimal Control Hemisphere Publishing Co

New York

C E Buckley The application of continuum methods to path planning PhD thesis

Stanford University Stanford CA

D Bullo ck and S Grossb erg Neural dynamics of planned arm movements Emergent

invariants and sp eedaccuracy prop erties during tra jectory formation Psychological

Review

k

M Camarinha F Silva Leite and PCrouch Splines of class c on nonEuclidean

spaces IMA J Math Control Inform

J F Canny The complexity of robot motion planning MIT Press Cambridge MA

J Cheeger and D G Ebin Comparison Theorems in Riemannian Geometry North

Holland Publishing Company Amsterdam

C H Chen and V Kumar Motion planning of walking rob ots in environments

with uncertaintyInProceedings of International ConferenceonRobotics and

Automation Minneap olis MN

IM Chen and J W Burdick A qualitative test for Nnger forceclosure grasps on

planar ob jects with applications to manipulation and nger gaits In Proceedings of

International ConferenceonRobotics and Automation pages Atlanta

GA

Y Chen and A A Desro chers A pro of of the structure of the minimumtime control

law for rob otic manipulators using a hamiltonian formulation IEEE Transactions

on Robotics and Automation June

PCrouch and F Silva Leite The dynamic interp olation problem on Riemannian

manifolds Lie groups and symmetric spaces J Dynam Control Systems

O Dahl Path constrained motion optimization for rigid and exible joint rob ots

In Proceedings of International ConferenceonRobotics and Automationpages

Atlanta GA

A Divelbiss and J T Wen Nonholonomic path planning with inequality constraints

In Proceedings of the nd IEEE ConferenceonDecision and Contr ol pages

San Antonio TX

M PdoCarmo Riemannian geometry Birkhauser Boston

B R Donald J Jennings and D Rus Information invariants for distributed ma

nipulation In K Goldb erg D Halp erin JC Latomb e and R Willson editors

Algorithmic Foundations of Robotics pages A K Peters Ltd Wellesley

MA

M Erdmann and M T Mason An exploration of sensorless manipulation In

Proceedings of International ConferenceonRobotics and Automationpages

San Francisco CA April

R Featherstone Robot Dynamics AlgorithmsKluwer Academic Publishers

A G Feldman Functional tuning of the nervous system with control of movementor

maintenance of a steady p osture ii controllable parameters of the muscles Biozika

AGFeldman Change in the length of the muscle as a consequence of a shift in

equilibrium in the muscleload system Biozika

C Fernandes L Gurvits and Z X Li Optimal nonholonomic motion planning for a

falling cat In Z Li and J F Canny editors Nonholonomic motion planningpages

Kluwer Academic Publishers Boston

P M Fitts The information capacityofhuman motor system in controlling the

amplitude of movement Journal of Experimental Psychology

T Flash and N Hogan The co ordination of arm movements An exp erimentally

conrmed mathematical mo del The Journal of Neuroscience

W H Fleming Functions of several variables SpringerVerlag New York nd

edition

G Garvin Tra jectory formulation in human movement An extension of existing

mo dels for single arm motions to coupled motions of two arms Masters thesis

UniversityofPennsylvania Philadelphia PA

G J Garvin M Zefran E A Henis and V Kumar Twoarm tra jectory planning

in a manipulation task Submitted to Biological Cyb ernetics

Q J Ge and B Ravani Computer aided geometric design of motion interp olants

ASME Journal of Mechanical Design

K Goldb erg Orienting p olygonal parts without sensors Algorithmica

Sp ecial Issue on Computational Rob otics

H Goldstein Classical Mechanics AddisonWesleyCambridge MA

J Gregory and C Lin Constrained optimization in the calculus of variations and

optimal control theoryVan Nostrand Reinhold New York

R L Grossman A Nero de A PRavn and H Rischel Eds Hybrid systems

volume of Lecture notes in computer science SpringerVerlag New York

J M Herve Intrinsic formulation of problems of geometry and kinematics of mech

anisms Mechanism and Machine Theory

M R Hestenes A general problem in the calculus of variations with applications to

paths of least time Research Memorandum RM The RAND Corp oration

M R Hestenes Calculus of variations and optimal control theoryJohnWiley

Sons INc New York

M R Hestenes Multiplier and gradient metho ds Journal of optimization theory

and applications

E C Hildreth and J M Hollerbach The computational approach to vision and

motor control Technical rep ort MIT AI Lab oratory August

M W Hirsch and S Smale Dierential equations dynamical systems and linear

algebra Academic Press New York

N Hogan An organizing principle for a class of voluntary movements Journal of

Neuroscience

J Hoschek and D Lasser Fundamentals of Computer AidedGeometric DesignAK

Peters

HP Huang and N H McClamro ch Timeoptimal control for a rob ot contour

following problem IEEE Transactions on Robotics and Automation

D H Jacobson Dierential dynamic programming metho ds for solving bangbang

control problems IEEE Transactions on Automatic ControlAC

D H Jacobson and M M Lele A transformation technique for optimal control prob

lems with a state variable inequality constraint IEEE Transactions on Automatic

ControlAC

D H Jacobson M M Lele and J L Sp eyer New necessary conditions of opti

mality for control problems with statevariable inequality constraints Journal of

mathematical analysis and applications

M E Kahn and B Roth The nearminimumtime control of op enlo op articulated

kinematic chains ASME Journal of Dynamic Systems Measurement and Control

September

A Karger and J Novak Space Kinematics and Lie Groups Gordon and Breach

Science Publishers

M Kawato Optimization and learning in neural networks for formation and control

of co ordinated movement Technical Rep ort TRA ATR Auditory and Visual

Perception Research Lab oratories Kyoto Japan

S W Keele Behavioral analysis of mov ement In V B Bro oks editor Handbook of

Physiology pages American Physiological So ciety Bethesda MD

O Khatib Realtime obstacle avoidance for manipulators and mobile rob ots Inter

national Journal of Robotics Research

KHHunt Kinematic Geometry of Mechanisms Clarendon Press Oxford

C A Klein and CH Huang Review of pseudoinverse control for use with kine

matically redundant manipulators IEEE Transactions on Systems Man and Cy

bernetics SMC

D E Ko ditschek Exact rob ot navigation by means of p otential functions Some

top ological considerations In Proceedings of International Conferenceon

Robotics and Automation pages Raleigh NC

W Kohn A Nero de J B Remmel and Xiolin Ge Multiple agenthybrid control

carrier manifolds and chattering approximations to optimal control In Proceedings

of the rd IEEE ConferenceonDecision and Control pages Lake Buena

Vista FL

G Laerriere and H J Sussmann A dierential geometric approach to motion

planning In Z Li and J F Canny editors Nonholonomic motion planningpages

Kluwer Academic Publishers Boston

L S Lasdon S K Mitter and A D Warren The conjugate gradient metho d for

optimal control problems IEEE Transactions on Automatic ControlAC

JC Latombe Robot motion planningKluwer Academic Publishers Boston

JP Laumond P E Jacobs M Taix and R M Murray A motion planner

for nonholonomic mobile rob ots IEEE Transactions on Robotics and Automation

S Leveroni and K Salisbury Reorienting ob jects with a rob ot hand using grasp gaits

In International Symposium on Robotics Research pages Munich Germany

Z Li and J F Canny Nonholonomic motion planning Kluwer Academic Publishers

Boston

Z Li J F Canny and S S Sastry On motion planning for dextrous manipulation

Part The problem formulation In Proceedings of International Conference

on Robotics and Automation pages

H Lipkin and J DuyHybridtwist and wrenchcontrol ASME J of Mechanisms

Transmissions and Automation in DesignSeptember

J Loncaric Normal forms of stiness and compliance matrices IEEE Journal of

utomation RA Robotics and A

T LozanoPerez and M A Wesley An algorithm for planning collisionfree paths

among p olyhedral obstacles Communications of the ACM

K M Lynch and M T Mason Stable pushing Mechanics controllabilityand

planning In K Goldb erg D Halp erin JC Latomb e and R Willson editors

Algorithmic Foundations of Robotics pages A K Peters Ltd Wellesley

MA

B Ma and W S Levine An algorithm for solving control constrained optimal

control problems In Proceedings of the nd IEEE ConferenceonDecision and

Control pages San Antonio TX

D P Martin J Baillieul and J M Hollerbach Resolution of kinematic redundancy

using optimization techniques IEEE Transactions on Robotics and Automation

M T Mason Mechanics and planning of manipulator pushing op erations Interna

tional Journal of Robotics Research

D Q Mayne and E Polak An exact p enalty function algorithm for control problems

with state and control constraints IEEE Transactions on Automatic ControlAC

J M McCarthy AnIntroduction to Theoretic al Kinematics MIT Press

R B McGhee and D E Orin A mathematical programming approach to con

trol of joint p ositions and torques in legged lo comotion systems In Proceedings of

ROMANSY Symposium

A Miele and T Wang PrimalDual prop erties of sequential gradientrestoration

algorithms for optimal control problems Part General problem Journal of math

ematical analysis and applications

P Morasso Spatial control of arm movements Experimental Brain Research

R M Murray Nonlinear control of mechanical systems A lagrangian p ersp ective

In IFACSymposium on Nonlinear Control Systems Design

R M Murray Z Li and S S Sastry A Mathematical Introduction to Robotic

ManipulationCRC Press

R M Murray and S S Sastry Nonholonomic motion planning Steering using

sinusoids IEEE Transactions on Automatic Control

E Muybridge Animals in motion Dover Publications New York First

published

E Muybridge The human gure in motionDover Publications New York

First published

Y Nakamura and H Hanafusa Optimal redundancy control of rob ot manipulators

International Journal of Robotics Research

W L Nelson Physical principles for economies of skilled movements Biological

Cybernetics

A Nero de and W Kohn Mo dels for hybrid systems Automata top ologies stability

In R L Grossman A Nero de A PRavn and H Rischel editors Hybrid systems

pages SpringerVerlag New York

C P Neuman and A Sen A sub optimal control algorithm for constrained problems

using cubic splines Automatica

N J Nilsson A mobile automaton An application of articial intelligence tech

niques In Proceedings of the st IJCAI pages Washington DC

L Noakes G Heinzinger and B Paden Cubic splines on curved spaces IMA J of

Math Control Information

C ODunlaing M Sharir and C K Yap Retraction A new approach to motion

planning In Proceedings of the th ACM Symposium on the Theory of Computing

pages Boston

J P Ostrowski The Mechanics and Control of Undulatory Locomotion PhD thesis

California Institute of TechnologyPasadena CA

PRPagilla and M Tomizuka Control of mechanical systems sub ject to unilateral

constraints In Proceedings of the th IEEE ConferenceonDecision and Control

New Orleans LA

F C Park Distance metrics on the rigidb o dy motions with applications to mech

anism design ASME Journal of Mechanical Design

F C Park and R W Bro ckett Kinematic dexterity of rob otic mechanisms Inter

national Journal of Robotics Research

F C Park and B Ravani Bezier curves on Riemannian manifolds and Lie groups

with kinematics applications ASME Journal of Mechanical Design

R PPaul Robot Manipulators Mathematics Programming and Control The MIT

Press Cambridge

F Pfeier and R Johanni A concept for manipulator tra jectory planning IEEE

Journal of Robotics and Automation RA April

D L Piep er The kinematics of manipulators under computer control PhD thesis

Stanford University

E Polak Computational methods in optimization Academic Press New York

E Polak On the use of consistent approximations in the solution of semiinnite

amming optimization and optimal control problems Mathematical progr

L S Pontryagin V G Boltyanskii R V Gamkrelidze and E F Mishchenko The

mathematical theory of optimal processesInterscience Publishers New York

M J D Powell A metho d for nonlinear constraints in minimization problems In

R Fletcher editor Optimization pages Academic Press New York

W H Press S A Teukolsky WTVetterling and B P Flannery Numerical

Recipes in C Cambridge University Press Cambridge

R Pytlak and R B Vinter Secondorder metho d for optimal control problems with

state constraints and piecewiseconstantcontrols In Proceedings of the th IEEE

ConferenceonDecision and Control New Orleans LA

E Rimon and D E Ko ditschek Exact rob ot navigation using articial p otential

functions IEEE Transactions on Robotics and Automation

R M Rosenberg Analytical Dynamics of Discrete SystemsPlenum Press New

York

H Sagan Introduction to the calculus of variationsMcGrawHill New York

C Samson and K AitAb derrahim Feedbackcontrol of a nonholonomic wheeled

cart in cartesian space In Proceedings of International ConferenceonRobotics

and Automation pages Sacramento CA April

DH Sattinger and OL Weaver Lie groups and algebras with applications to physics

SpringerVerlag New York

B F Schutz Geometrical Methods of Mathematical Physics Cambridge University

Press Cambridge

A L Schwartz Theory and implementation of numerical methods basedonRunge

Kutta integration for solving optimal control problems PhD thesis Universityof

California at Berkeley Berkeley CA

J T Schwartz and M Sharir On the piano movers problem The case of

twodimensional rigid p olygonal b o dy moving amidst p olygonal barriers Communi

cations on pure and applied mathematics

J T Schwartz and M Sharir On the piano movers problem General techniques

for computing top ological prop erties of real algebraic manifolds Advances in applie d

mathematics

Z Shiller On singular p oints and arcs in path constrained time optimal motions

ASME Dyn Syst Contr Division DSC

Z Shiller Timeenergy optimal control of articulated systems with geometric path

constraints In Proceedings of International ConferenceonRobotics and Au

tomation San Diego CA

Z Shiller and S Dub owsky On computing the global timeoptimal motions of rob otic

manipulators in the presence of obstacles IEEE Transactions on Robotics and Au

tomation December

K G Shin and N D McKay Minimumtime control of rob otic manipulators with

geometric constraints IEEE Transactions on Automatic ControlAC

K Sho emake Animating rotation with quaternion curves ACM Siggraph

S Singh and M C Leu Optimal tra jectory generation for rob otic manipulators

using dynamic programming ASME Journal of Dynamic Systems Measurement

and Control

JJ E Slotine and H S Yang Improving the eciency of timeoptimal path

following algorithms IEEE Transactions on Robotics and Automation

February

J F So echting and F Lacquaniti Invariantcharacteristics of a p ointing movement

e in man The Journal of Neuroscienc

E D Sontag and Y Lin Gradienttechniques for systems with no drift In Proceed

ings of Conference on Signals and Systems Princeton NJ

E D Sontag and H J Sussmann Timeoptimal control of manipulators In Proceed

ings of International ConferenceonRobotics and Automation pages

San Francisco CA April

R B Stein What muscle variables do es the nervous system control in limbmove

ments The Behavioral and Brain Sciences

K Sugimoto and J Duy Application of linear algebra to screw systems Mechanism

and Machine Theory

K C Suh and J M Hollerbach Lo cal versus global torque optimization of redun

dant manipulators In Proceedings of International ConferenceonRobotics and

Automation Raleigh NC

H J Sussmann Lie brackets real analyticity and geometric control In R Millman

R W Bro ckett and H J Sussmann editors Dierential Geometric Control Theory

pages Birkhauser Boston

H J Sussmann and W Liu Lie bracket extensions and averaging The single

bracket case In Z Li and J F Canny editors Nonholonomic motion planning

pages Kluwer Academic Publishers Boston

K L Teo C J Goh and K H Wong A uniedcomputational approach to optimal

control problems Longman Scientic Technical Burnt Mill Harlow UK

D M Tilbury Exterior dierential systems and nonholonomic motion planning

PhD thesis University of California at BerkeleyBerkeley CA

Y Uno M Kawato and R Suzuki Formation and control of optimal tra jectory in

human multijointarmmovement Biological Cybernetics

F A Valentine The problem of lagrange with dierential inequalities as added side

conditions In Contributions to the Calculus of Variations pages Chicago

University Press Chicago IL

P Viviani and C Terzuolo Tra jectory determines movement dynamics Neuro

science

M Vukobratovic and M Kircanski A metho d for optimal synthesis of manipulation

rob ot tra jectories ASME Journal of Dynamic Systems Measurement and Control

K J Waldron Geometrically based manipulator rate control algorithms Mechanism

and Machine Theory

G W alsh DTilbury R Murray and J P Laumond Stabilization of tra jectories for

systems with nonholonomic constraints IEEE Transactions on Automatic Control

CC Wang Local and Repeatable Coordination Schemes for Redundant Manipula

tors PhD thesis UniversityofPennsylvania Philadelphi a PA

J Wei and E Norman On global representations of the solutions of linear dierential

equations as a pro duct of exp onentials Proc American Mathematical Societypages

April

D E Whitney Resolved motion rate control of manipulators and human prostheses

IEEE Transactions on ManMachine Systems MMS

D E Whitney The mathematics of co ordinated control of prosthetic arms and ma

nipulators ASME Journal of Dynamic Systems Measurement and Control

David A Winter Biomechanics and motor control of human movement John Wiley

Sons New York second edition

M Zefran Numerical metho ds for computing optimal tra jectories of rob ots Masters

thesis UniversityofPennsylvania Philadelphi a PA

M Zefran J Desai and V Kumar Continuous motion plans for rob otic systems

with changing dynamic b ehavior Pro ceedings of nd Int Workshop on Algorithmic

Foundations of Rob otics

M Zefran and V Kumar Optimal control of systems with unilateral constraints

utomationpages In Proceedings of International ConferenceonRobotics and A

Nagoya Japan

M Zefran and V Kumar Co ordinatefree formulation of the cartesian stiness

matrix In Proceedings of the th Int Symposium on Advances in Robot Kinematics

Portoroz Slovenia

M Zefran and V Kumar Planning of smo oth motions on se In Proceedings of

International ConferenceonRobotics and Automation pages Min

neap olis MN April

M Zefran V Kumar and C Croke On the generation of smo oth threedimensional

rigid b o dy motions Submitted to IEEE Transactions on Rob otics and Automation

M Zefran V Kumar and C Croke Choice of Riemannian metrics for rigid b o dy

kinematics In Proceedings of the ASME th Biennial Mechanisms Conference

Irvine CA

M Zefran V Kumar J Desai and E Henis Twoarm manipulation What can we

learn by studying humans In Proceedings IROS Pittsburgh PA August

M Zefran V Kumar and X Yun Optimal tra jectories and force distribution for

co op erating arms In Proceedings of International ConferenceonRob otics and

Automation pages San Diego